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ABSTRACT 


Optimal estimation techniques are manipulated to demonstrate the 
feasibility of using a Wiener filter for trim analysis of a submarine. 
The essence of the problem is the estimation of deviations of the sub- 
marine's weight and longitudinal! center of gravity from those values 
which enable the submarine to maintain ordered depth and angle of 
pitch with minimal control surface deflections. 

Linearized equations of motion are developed and put into state 
vector notation. The state vector itself contains the variables which 
are to be estimated. Optimal! control techniques are used to design a 
linear regulator which is used to control the simulated submarine in 
the vertical plane. Two types of filters are designed. The first is 
sub-optimal in that its gains are derived by pole placement techniques 
which are used to ensure filter stability. The second filter is a 
Wiener filter, in that its gains come from the steady state solution 
of the matrix Riccati equation. A least squares smoother is designed to 
smooth filtered estimates of the variables of interest. 

Simulations on a digital computer are used to demonstrate the 
ability of the filters to estimate the submarine's state of trim in the 
presence of disturbances to the submarine and the measurement instru-~ 
ments. It is also demonstrated that the Wiener filter is capable of 
estimating the severity of a flooding casualty on the submarine. 
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Figure 1. Illustration of Plane of Symmetry and Positive 
Directions of Axes, Angles, Velocities, Forces and Moment, 
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Pecreneral 

Over the past few decades, many technological advances have been 
made inthe design and construction of submarines. They are traveling 
faster, deeper, and for longer periods of time than ever before. 
Nontheless, there are many things that remain the same. Naturally, 
the physical laws governing the behavior of submarines in hydrospace 
have not been altered by man's more adept manipulations. Tobe 
Specitic submarines are still subjected to hydrostatic and hydrodynamic 
forces, the magnitude and direction of which are dictated by the design- 
er's appreciation for the natural law: Another unaltered fact is that 
these undersea craft are manned by sumbariners whose fates lie largely 
in their respect for the sea that surrounds them Tragedies of the past 
bare stark evidence to how intolerant of error the sea canbe. Yet it 
is said, 'Toerris human." Designers, in their appreciation of this 
fact, strive for safety through factors of safety and various devices 
designed to protect the submarine and her crew. Submariner's seek 
refuge in exhaustive training, practice, and vigilance indeed, the 
results have been impressive. It has been the experience of the Author 
and most submariners that serious mishaps are seldom precipitated 
by any one mistake or mechanical failure. Post accident investigations 
most often reveal a chain of events which led to a disturbing, if not 
disasterous, culmination. 

Imagine, for example, a nuclear submarine making a high speed 
submerged transit. It has been going fast for several hours, and un- 
known to the crew, it has become grossly heavy, or out of trim as 
submariner's are apt to say. The pitch angle of the boat and the control 
surface deflections have been monitored closely, but their slight 
deviations from neutral angles and the ensuing large hydrodynamic 
forces remain undetected. Suddenly, main propulsion is lost, and 


emergency power is channeled to auxiliary systems while the submarine 





begins to slow and the engineering department works hurridly to 
restore main power. At the diving stand, it soon becomes apparent 
P@etemiuch greater deflections of the control surfaces and pitch angle 
are required to maintain depth. The diving officer, realizing that the 
boat is heavy, gives orders to commence pumping variable ballast 

to sea. The officer of the deck quickly grasps the gravity of the situ- 
ation and orders an emergency surfacing. The control surfaces go 

to maximum deflections in an effort to drive the boat toward the surface 
while air valves are Opened to blow main ballast tanks. Unexpectedly 
the air lines rupture and emergency power is lost. In the dim light 

of the battle lanterns panic begins to take hold as the men sense the 
rising pressure of the atmosphere against their eardrums. Having 
lost its initial momentum, the submarine reaches an apex and then 
begins to descend, slowly at first but then more and more rapidly. 

The officer of the deck has had difficulty making himself understood, 
but finally the main ballast blow is secured and the air andtrim systems 
are lined up in a desperate effort to blow variable ballast to sea. The 
process seems to take forever and the diving officer gives the count- 
down as he announces the increasing depth inten foot increments. The 
submarine is well beyond test depth and into the Jesus Factor’ before 
the lineup is completed and blowing commences. It is unfortunately 
moeelittle and too late. 

It must be admitted that such an occurance is highly unlikely, but 
statistics over the past eleven years indicate that disasterous chains of 
events are not impossible. This scenario would have ended differently 
had the submarine not been out of trim initially. The purpose of this 
thesis is to derive and test an algorithm for a trim filter, that is, an 


electronic filter capable of trim analysis. 





fee erim Analysis 

In the way of background, trim analysis is one of those facets of 
Ssubmarining that has been affected little by recent technological 
advances. It is the skill of estimating the required distribution of variable 
ballast to get the submarine in an "in trim" condition. Such a condition 
has been achieved when the submarine is capable of maintaining ordered 
depth with an ordered angle of pitch and with minimal deflections of 
the control surfaces. 

The ordered angle of pitch is most commonly referred to as 
"ordered bubble’ and sometimes ordered trim angle The term bubble 
derives from the inclinometers used on older submarines. The instru- 
ments were inverted ''U'" shaped glass tubes nearly completely filled 
with a colored fluid. A small air bubble always found its way to the 
highest point in the tube. An incremented scale behind the tube indicated 
the angle at which the instrument was inclined. Since they were aligned 
inafore and aft direction, the measured angle (or bubble if you will) 
coincided with the boat's angle of pitch. 

Sometimes, evolutions within the submarine dictate the ordered 
bubble. For example, while servicing torpedoes, it is frequently nec- 
essary to unlash torpedoes in their skids or raise the stop bolts which 
restrain them in torpedo tubes so that they may be loaded or unloaded. 
During the time that the weapons are free to travel longitudinally, it is 
wise to trim for a zero bubble so that the torpedomen need not wrestle 
unnecessarily with the forces of gravity. However, the prime candidate 
for ordered bubble is the so called ‘neutral’ bubble. As the submarine 
changes speed, hydrodynamic forces and moments change in proportion 
to the velocity squared. When the submarine is at the neutral bubble, 
these changes are in equilibrium, and it is neither necessary to change 
the wieght of the submarine nor to shift the center of gravity. 

Necessary changes of variable ballast are accomplished with the 


trim system. Basically, it consists of tanks located throughout the 





boat. The forward trim tank is in the vicinity of the bow; after 

trim tank, near the stern; and the auxiliary tanks near midships, 
number one to starboard and number twoto port. All tanks are con- 
nected to a trim manifold for which there are also water lines to and 
from a trim pump and the sea. Through the correct combinations of 
opened and closed valves at the manifold, it is possible to control the 
transfer of variable ballast. 

As was implied in the scenario, it is the duty of the diving officer 
to keep the submarine on ordered depth and, towards that end, to keep 
the submarine intrim. He observes the actions of the planesmen in 
their effort to maintain ordered depth and ordered bubble. Due to 
external disturbances and natural tendencies of the planesmen and the 
submarine, the planes are seldom stationary, nor is the bubble. The 
diving officer therefore mentally estimates the averages of those angles. 
Based upon those average angles, he then makes an estimate of the 
state of his trim, which is more qualitative than quantitative. Ifhe 
deems it necessary, he issues commands to the trim manifold operator 
to correct errors inthe submarine's weight (W) or LCOG(x/.). Table 
I illustrates possible states of trim. ‘'Stick'’ diagrams, such as that 

State 
ie rim W = Wo; ¥G= X Go 
fmeavy Overall (HOA) W >Wo 
Light Overall (LOA) W< Wo 
Heavy Forward (HF) HOA AG = XC5 
Heavy Aft (HA) Ore = x, 
Light Forward (LF) DOA = = XCo 
Light Aft (LA) LOA, XG >XGo 
Heavy Forward, Light Aft W = Wo, XG> XGo 


Light Forward, Heavy Aft RG one ee 
Alright Fore and Aft XG = XG, HOA or LOA 
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iaple em atares Of Prim 
shown in Figure 2, are useful in representing the estimate of averaged 


angles. For this particular illustration, it is assumed that 





bow or 
stern 
airwater 


planes ~= hul| Va 3 eee 





Pisure 2. “otick' Diagram 


the pitch angle is near neutral so that it contributes nothing tothe state 
of equlibrium. The fairwater planes impart an upward force anda 
positive pitching moment, while the stern planes impart a downward 
force and a positive pitching moment. The net effect is a force near 
zero in the vertical direction and a positive pitching moment. The 
submarine is therefore heavy forward, light aft. The correct command 
would be Pump from forward trim to after trim.’ The resulting shift 
aft of the center of gravity would provide the necessary hydrostatic 
pitching moment, so the planes should return to zero deflections . 
During the pumping operation, the diving officer would monitor the planes 
to ensure his analysis had been correct and to issue the command, 


"secure pumping when the submarine was in trim. 


3. Factors Affecting a Submarine's Trim 

Once the diving officer has achieved the state of bliss known as 
being in trim, he will not be able to relax for the remainder of his 
watch. For unfortunately, the state of trim of a submarine is always 


ina state of change. Onthe brighter side, the rate of change is usually 





slow. 


First, there are changes within the submarine. Provisions are 
consumed, water is distilled and consumed, and Sanitary tanks 
are filled and evacuated. Bilges fill and are pumped. The crew is in 
a perpetual state of motion. Many an officer student has been victim- 
ized by a "“trimm ing party composed of his classmates. As the 
student diving officer concentrates under the watchful eye of his in- 
structor, members of the trim party file by inconcpicuously from one 
end of the boat to the other. Just as orders are Pivente pump £fom 
forward trim to after trim, the trim party commences its migration 
from the bow tothe stern. It is a wise diving officer who monitors 
planes, angle, and passageway. 

Environmental factors affect the submarine's trim also. One rel- 
evant variable is the sea water density. As the submarine passes from 
one density to the next, the hydrostatic buoyant force changes and comp- 
ensation must be made. Near surface effects constitute another problem 
area. The boat is subjected to an abundance of external disturbances. 
There is an apparert suction between the boat and the surface for which 
compensation must be made. Deviations from mean angles become more 
severe for the planes and hull. Plane deflection indicators do not reveal 
the true angles of attack of the planes as nearby surface waves atiect 
water particle velocities about the submarine. Under such circumstances, 
mental averaging Secomes m.vore aifficult and trimi analysis more clouded 
As one of Antony's oarsmen lamented to a comrade at the Battle of 


Actium, ‘There must be a better way!’ 


foe Proposal 
The present technique of trim analysis could at best be described 
as adequate. The advent of nuclear propulsion has highlighted two of 


its major shortcomings. First, as speed increases, hydrostatic im- 


balances become difficult, if not impossible. to perceive. Secondly. 
despite the increase in demand on their analytical abilities, diving 
Officers find less time to learn and practice the art of trim analysis 
due the enormous demands imposed by nuclear propulsion. Never 

has it been so difficult or time consuming to master the art of submar- 
famaeern all of its aspects. | 

In the way of improvement, one suggestion has been to design a 
device which automatically computes averages for the plane deflections 
and angle of pitch. While this is credible in its simplicity, it falls 
short of the mark while the submarine is going through transients in 
depth. It was indeed an exceptional diving officer who could estimate 
variations in the trim as his boat was changing depth. 

The solution offered by this thesis is the trim filter. It is a device 
which estimates the response of the submarine to bounded disturbances 
and measured plane deflections. If the actual response differs from the 
estimate, the filter revises its estimate of the state of trim. The result 
Peeaecontinuous process of trim analysis. As it turns out, a trim filter 


is not as complex as one might expect 
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a. General 

Optimal estimation techniques were investigated to find a method 
which was capable of the dynamic prediction and estimation mentioned 
in the previous chapter. The Kalman filter seemed to be an ideal 
solution. Basically, the procedure used was 

Me derivation of a mathematical model, 

ee controller design, 

3. filter design, and 

4. computer simulation. 

Most of the details on the above steps appear elsewhere in the 


thesis. However, a brief description follows. 


a State Vector Model 


Kalman filtering requires that the system be described by a lineari- 
zed model in state vector notation. As described in reference (5), the 


general form for such a model is 


d/dt x(t) = A(t) x(t) + B(t) u(t) (I1-1) 


where 


x(t) = a state vector of n variables which completely describe 
the state of the system at any instant in time, 


u(t) a Control vector Of r variables, 
A(t) = ann X n system coefficient matrix, and 


B(t) = ann X r control coefficient matrix. 


It can be seen that the rows of the state vector model are merely 
n first order differential equations. The output of the system is 


described as 





ll 


y(t) = C(t) x(t) (Ii=2) 


where 


y(t) = an output vector of m variables, and 


C(t) = anm Xn output coefficient matrix. 


The most obvious candidates for variables in the state vector 
are depth (2, descent rate (2 Hitch (9), pitch rate (ein and forward 
velocity (u). Usually, terms related to the mass and center of gravity 

of such a system are included in A(t). However, the nature of the trim 
problem is that these values are unknown. Rather than use classical 
parameter estimation techniques to compute the values of the coefficients, 
it was decided to include the submarine weight (W) and longitudinal 


center of gravity (x_.) in the state vector.| This serves two purposes. 


oS 
First and foremost, Kalman filtering facilitates the estimation of comp- 
onents of the state vector for which no direct measurements are available. 
Hence, a filter can be used to estimate the unmeasurable variables W and 
Xa: Secondly, the solutions and simulations are for constant propeller 
shaft rpm. Since ordered velocity is constant and variations in the trim 


are accounted for inthe state vector, it can be assumed that the coefficient 


matrices are constant. Hence, equation (II-1) becomes 


adi x(t)=A x(t) +B u(t) (I1-3) 


and equation (II-2) becomes 
y(t) = C x(t) (II- 4) 
Finally, control surface deflections are included in the state 


vector. This is to facilitate the controller design since actually the 


rates of plane deflections are the control variables in depth keeping. 
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Appendix A describes the details of the derivation and linearization 


of the equations of motion. This is done about an equilibrium state 


(Appendix B), so that the components of the state vector are the devi- 


ations from equilibrium. The complete state vector is therefore 


Ss i= 2) 


Notice that equation (II-3) implies that the coefficients of x(t) on 
the leit hand side is the identity matrix, I. However, the form cd the 


equations after the linearization is 
d/dt (Vx) = Wx + Zu + ZZ F (II-6) 


where, using the nomenclature of Appendix A, 


Fos f 7 ext 
= ss 
oa Wve Ni ext ({I- 7) 
-- 
Bee oe ext 


UW = = (mes) 


and the coefficient matrices are 
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(II-11) 


IN 

if 
oy (ey Mejor aS io) (|S) 
C26) Cy Fe Se ae 


(II-12) 


IS 

i 
SOLOS Sco s-7> 
S21O'S O° @.ote 
co sR ca > Ae OE nN ao a Soa 


Although this format appears cumbersome, it is a muSt for the 
steps which follow. The second, fourth and seventh rows of the matrix 
equations are simply the linearized heave, pitch, and surge equations 
of Appendix A. Ina more straight forward notation, the remaining 


equations are 


d/dt aa = Ze (I-15) 
d/dt ©, = Ge (11-14) 
d/dt SL= (I-15) 
d/dt §. = 9, (I-16) 
qWdtex. =O (iene 


Ge 


d/dt Ww =O (II-18) 
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Multiplying both sides of equation (II-6) by the inverse of V 


makes the equation resemble the required form. 


pea Ae + Bo ut BS" (II-19) 
where 
ag 
=I 
B=-V 2 Abies 
and 
=I 
Bi=V ZZ (IT-22) 


The last term of equation (II-19) is merely a means of introducing 
steady state forces and external disturbances during the actual simu- 


lations. 


Dh Depth Controller Design 

In order to keep the simulated submarine on ordered depth in the 
presence of disturbances and to effect depth changes, it was necessary 
to design a depth controller. Modern optimal control techniques were 
used rather than classical techniques for two reasons. First, having 
derived a state vector model of the submarine, the equations were al- 
ready in the proper format for finding optimal feedback gains. Second, 
as is demonstrated in the appendices, the optimal control problem and 
the Kalman filter problem are solved by identical techniques. Hence 
the Author was able to capitalize on his experience with controller 
design when designing the filter. Also, the parallels between the two 
problems were instrumental in grasping the significance of the technique 
in its application tothe filter problem , with which the Author was here- 


tofore unfamiliar. 





De 





Figure 3. Optimal Controller Structure 


Figure 3 illustrates the assumed structure for the controller 


design. The problem was simplified by not feeding back Au, Xo a? and a 
We The state vector for the controller design was therefore 


oe 


N°N 


oe 
e 


é =2'3 
5 (iil ) 


as | 


and the model for determining controller gains was 


Rk 


O-O 


d/dtx =A x +B u (II-24) 
cae a Cavan © os, Cea 


where, in terms of the elements of the original matrix, equation 


(II-20 }, 


a 


18 


ioe i iow 14) 15) 16 (11-25) 
ieee 23 -24°° 25 "26 
Ale 330 34 35 "36 
Al “42 “43 “44 “45 “46 


Simoes 25 54 557.56 


(ie Ge) 
| 


The details of the manner in which the feedback gains, K 


Eieeas 1t turns out, 


OD 

T 
Rect Ye ao pee >) 
ea One) Oe) 


3 


Cc 
were computed are discussed in Appendices C and E. Stated briefly, 


a quadratic performance index was selected and the gains emerged 
from the solution of the matrix Riccati equation. Potter's method was 
found to be the most effective method of solution, regardless of the 
weighting matrices in the performance index. 

If is re-emphasized that the controller design is not the object of 


this thesis. It was merely a necessary step inthe computer simulation 


of the submarine dynamics. 


4, Filter Design 

Figure 4 illustrates the basic relationship between the Kalman filter 
and the reduced physical system. Detailed descriptions of filters 
appear in references (5), (7), and (8). The general aspects are discussed 


below. 
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Figure 4. Kalman Filter Structure 


The purpose of the Kalman filter is to make a maximum likelihood 
estimate of the state of a system where variables are inaccessable and/or 
external and measurement noises are present. The theory is based 
upon the assumption that the disturbances are in the form of white noise 
with known statistical properties. This is rather a strict assumption. 

Its impact on the actual filter depends upon how wide the system's 
bandwidth is compared with the disturbances’. The system equation 
1s 


te eee Bele, tS) (1-27) 
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where Up is a deterministic input and w, a random disturbance. To 


simplify the filter design, the filter state vector used was 


Xp : e (II-28) 


for which (in terms of equation [I-20) 


iso ie 14 49 A109 


Soy 22.23. “oa 793 a 
29 
a Se 208 28 iessd 7398 436 
=f: 
oie 42743 “a4 748 “45 (II-29) 
TM aso ose “a4 ag. 786 


[Rem O2 ue G3, “g4. “999 99) 


Two input vectors were used. The first was 


aL 
4. 


Uo, = CAF ont) 


E a -l 


au 


(II-30) 


For which(in terms of equations [I-20 and 22) 





Tani 


0 0 0 0 0 
295 896 D9; Plog oq 
0 0 0 0 0 
ae "45 746 «(7 41 a2 944 eee 
0 0 0 0 0 
0 0 0 
The second input vector used was 
Si, 
ds 
E(F Sot) 
E(F A, eee 
A 
uae a (= 325 
—t2 Ax 
Ge 
AW . | 
For which 
0 0 0 0 0 0 
! t 
Ao 5 256 b 2] b 29 Qo 7 O 
B _ 0 0 0 0 0 0) 0 (11-33) 
—f2 a Q b! b! a 0 0 
45 46 4} 42 47 
0 0 0 0 0 l 0 
0 0 0 0 0 0 ] 


Two sets of variables were assumed to be measurable, The first 
set is based on the assumption that the submarine is equipped with a 
nS oe 
oe” oe e 
matrix would have gains suitable for converting 


modern inertial navigation system capable of measuring z 
and 9 The real C, 
Signal voltages from various instruments to signals corresponding to the 


actual values of the measured variables. Without jeopardizing the validity 








of the results, it was assumed that signals had already been converted 
so that 


(I1-34) 


a 
i. Go 
ce 
oO a a S 
— OF ouke 
CO 3) oS 
SS © @ & 


The purpose of the second set of variables used was to ascertain the 
validity of the application of the filter to more conventionally equipped 
submarines, In such cases there is no instrument for measuring z 


directly, so that 


1 0 0 
ee =| 0 oO 1 0 o 0 (II-35) 
0 0 1 


These matrices are used to describe the measurement signal, 


which is 


=C tov (11-36) 


ei 
According to theory, if the n X nm composite matrix 


: | —— 
lc em atc A VeT| (11-37) 


is of rank n, then the system is observable, ie., a filter can be desiged., 
After the system was properly scaled, the above mentioned test was 
performed successfully for the first input vector (Up). The scaling pro- 
vided for all angles to be measured in degrees, z.,, and x, In feel 


oa in degrees/ second, Zane in feet/second, and we in megapounds., 





ae 


The filter equation is 


A A . A 
d/dt Xp = Ag X¢ eer Us + Kz, - Y ¢) (11-38) 
where 
A IS 
Ye = Cp Xp (i=39)) 
The ideal means of calculating the feedback gains matrix, K,, are 


5 
described in Appendices D and FE. These techniques were first applied 


to a system with input Us) (equation JI-30),. In practice, no solution was 
possible by the integration technique (time steps as small as 0.01 seconds 
were attampted), and Potter's technique gave only a partial solution, the 
first four rows of the gain matrix. Hence, all that would have been 
possible was estimating the measureable variables. The object of this 
thesis being to estimate Xa. and Wey it was essential to find gains for the 
fifth and sixth rows of K; 


from Potter's method, supplemented by educated guesses. The tech- 


This was done by using the positive results 


nique used is described in Appendix D. It was basically a pole placement 
technique. The final result was a filter that was two thirds optimal 
and one third ''tweaked, "’ 

Subsequently, it was discovered that the problem was amenable to 


Potter's method if the second input vector (equation II-32), was used. 


u 
° ={2 
imme inclusion of the artifical noises, A Xa6 and AW, provided a 

mathematical means for the variation of the related parameters, Hence, 


gains could be calculated for compensation using the "optimal" technique, 


D. Simulation 

All simulations were performed on a IBM 360 computer. After the 
matrices had been set up, the differential equations were solved by a fourth 
order Runge-Kutta method, Hamming's modified predictor - corrector 
method was attempted first, but variables whose differentials were 
zero tended to "drift'' rather than remain constant as they should have. 


A time step of 0.1 seconds was used to make variables changed 
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between time steps, such as controller or filter feedback and distur- 
bances, behave ina manner closely resembling continuous functions. 

The linearized equations of motion were used to simulate the 
submarine dynamics, and the density of the sea water and the acceler- 
ation due to gravity were assumed to remain constant. Figure 5 de- 
picts a block diagram of the simulation program which appears in Appendix 
F. The gains matrices, K. and K, , were calculated as described in 
the two previous sections. Both rate and position constraints were im- 
posed onthe planes. 

Preliminary simulations were for the purpose of testing controller 
gains for given depth changes. Filter gains were tested for step inputs 
without and with Gaussian noise. Sensitivity to a ramp input ( flooding 
casualty) was tested and a least squares technique for smoothing was 
tested. All estimates were made while the submarine was periodically 
changing depth. 

The coefficients used appear in Table Il. They were derived by 
iMiadepulating data from a motley collection of books, reports, and articles 
on fluid drag, potential theory, bodies of revolution, airfoils, sub- 
mersibles, airships and naval propulsion. Although they represent no 
real submarine, the results obtained with them seem realistic, based 
upon the experience of the Author. It is therefore assumed that they are 
satisfactory for the sake of a numerical example and that the procedures 
described herein could be successfully applied to an actual set of sub- 
marine coefficients. The means of converting from non-dimensional 
form can be found in references (2) or {3)rmthe computer program for 


Simulations in Appendix F. 
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1. "In Trim'' Condition 


Figures 6 and 7 depict the solutions of equations (B-14) and 
(B-16) for Me ay qtel Xa 


ordered bubble (0). It can be seen in figure 6 that a horizontal line 


as a function of command speed (u) and 


drawn through W-B = 0 would le between ©. = -l and 2S -2° | 


hence 


-2 < Oneutral * - 1 (111-1) 


Looking at figure 7, it is seen that all curves are parabolic and 
that those in the vicinity of the bound expressed in (III-1) intersect 
at about 7.15 knots. It 1s safe to assume that the curve for the neutral 
bubble would be a horizontal line passing through the intersection at 


7.15 knots, Therefore 


eeeutral 02025 it (III-2) 


At zero speed where the only forces are hydrostatic, 1t can be seen 
that for each degree of trim, there corresponds a shift in XG of about 
.0185 feet. This fact can be used to linearly interpolate bet ween a 


and -2°: 


=) Nim oes 


neutral 


° Xcneutral - *Gi 
poi eee. (IIT -3) 


CU Wee 


or 


eo / 0,025 - 0.0195 


oO 
= ep 
2. ae | 28 wt 


0,039 - 0.0190 


For the simulations, it was decided to use the nearest integer to the 


neutral bubble for ordered bubble, hence 


GO = Sa for simulations, 
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Figure 7 is useful to demonstrate another point. Below 7.15 
knots, trimming to pitch the bow down requires shifting the center 
of gravity forward as one might expect. However, above that speed 
the opposite is true. This is due to the domination of hydrodynamic 
moment over the hydrostatic moment. Although the transition is 
gradual, it can be said that, in general, hydrostatics dominate below 
7.15 knots and that hydrodynamics govern above 7.15 knots. 

Since trim analysis is most difficult when hydrodynamics dominate, 
it was decided to test the filter well above the transition” velocity. 


Therefore a command speed of 12 knots was used for all the simulations. 


2. Controller Tests 
Two sets of controller gains were calculated. The weighting 


Mmiatrrces for the first were: 


100 0 0 O oO 9Q 
0 0 860 Oo 720 
0 0 4 0 0 0 
Se 0 0 0 0 0 0 oa 
0 0 OF -0:8 20:7 10 
0 0 0 0 O 0 
and 
1 0 
Pp as (IIT-6) 
—cl 0 i 
and the resulting gain matrix was 
8. 721 350) Jee -$.979 -34.53 -.6673 -, 008933 
=cl 
4,894 17.82 1,497 ols -,008932 -, 4408 


(III-7) 





2 


Figure 8 depicts the response of the ‘controlled '' submarine 
to an order to decrease depth by 50 feet. When last seen, the sub- 
marine was passing 4000 feet with a pitch angle of -116° ! Crews 
have jokingly given diving officers quarters for much less exciting 
rides. 

Going back to the weighting matrices, the logic in Qual was that 
maintaining depth was far more important than ordered bubble which 
was the only other variable worth weighing. Most diving officers would 
have pitched up about 5° for such a depth change, but the "optimal”’ 
controller which is actually a linear regulator does not account for 
the constraints on the planes' deflections, Even though the error in 
depth far exceeds the bubble error, the controller "thinks" it can 
change depth in an optimal manner while maintaining a pitch angle 
close to the ordered bubble by using plane deflections exceeding 180° if 
the need exists. This highlights two short-comings: the failure to 
secount for constraints and the failure of linear theory to account for the 
Setalime of control surfaces at large angles of attack, 

By the time Oo. became large, Le became much larger and was 
still the predominant error. With luck, this controller might have 
accomplished a depth change of 8 feet. 

An attempt was made to calculate a set of gains for which there was 
no weighing of pitch angle error, however, the Riccati equation was un- 
solvable for such a case which the techniques at the disposal of the 


Author, Therefore a compromise was made for the second set of gains, 


2. was weighted more heavily as was uc The matrices were: 
ooo 6D 
0 0 0 0 O 0 
0 O 400 0O 0 0 
Qo9 = 0 0 0 0 0 (IIT-8) 
0 860 0 0 0 | 
0 0 0 0 O- 
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: 100 0 
+ : (III-9) 
ee 0 100 
and 
a. 2999 2.996 -,7980 -6.049 -,2036 .082163 
—c2 .0068262 1.103 2.147 8.762 .082163 -.3585 


(III-10) 


Comparing K 9 with Le , One can see the decrease in the total 
control effort and the increase in the relative importance of Oo . over 
z .- Figure (9) shows the typical record of depth change orders 


oe 
and submarine response for the remainder of the simulations, 


See bilter Gains 

Four filter feedback gains matrices (Ky ) were calculated. The 
matrices and relevant data appear in tables III thru VI. Unless indicated 
otherwise, off-diagonal elements of square matrices are zero, 

Also note that jae ey and Au are treated as external disturbances, 
The noise assigned to these variables can be used to imply the un- 
certainty which exists in their physical impact on the submarine dy- 
namics. For example, it is the angle of attack of the control surfaces, 
rather than their deflections relative to the submarine, which determines 
the forces and moments they impart on the submarine. The greatest 


degree of uncertainty for these angles exists near the surface. 





Table III K amaemeltated Data 


fl 


o 


speed: 12 knots; Ordered Bubble: -1 
Input Vector: us) (equation II-30) 


Measurement Matrix: C,, (equation II-34) 


fl 
Technique: Potter/pole placement 


Variable E(w or v) Standard Deviation lees or os 
Sp 0 _ lf??2 degrees tr /100 
Ss 0 .1772 degrees Ar /100,, 
FZext 0 1,772, 45 pounds TX 10, 
F Mext 0 17, 724.5 foot pounds Tr X 10 
Au 0 .1063 feet/second s0113 
Zoe 0 .8862 feet . 7854 
Ze 0 , 8862 feet/second , 1854 
Ge 0 .1772 degrees 7 /100 
Ge 0 .1772 degrees/ second 7/100 
a /100 
47/100 6 
TX 10 9 
Qe) = TX 10 
- -Ols 
. 1854 
P z . 1854 
—f] F7/100 
t1 /100 
0,052295000 0.0015072000 -,0832560 0.002256200 
0.001507200 0.0001506800 -.0106650 -.000061439 
i 2 -,003330200 -.0004266000 0,0485490 0.001320300 
= Seow ougo24G § =.,0000024576 0.0013203 0.000225530 
0.000450000 0.0000200000 -0020000 -004500000 
0.000030000 0.0003200000 0.0000000 0, 000000000 | 
Figenvalues 
Real Part Imaginary Part 
-,. 1287 0.0 
a PUC304 
030 11 =, 06394 
-, 04259 BLURS yo/es: 
-, 04259 ani353 


-, 01234 0.0 





Table IV K and Related Data 


f2 
Boeed: i2 knots; Ordered Bubble: - i 
Input Vector: u,, (equation l{-30) 

Fa (equation II- 34) 
Technique: Potter/pole placement 


Measurement Matrix: c 


meictaple E(w or v) 


$b 


0 Ik (6 Fetes eters 
$s 0 1 degree 
FZext 0 1 pound 
F Mext 0 I foot pound 
Au 0 1 foot/second 
Zoe 0 . 03162 feet 
Zoe 0 . 03162 feet/second 
Ge 0 .03162 degrees 
Oe 0 .03162 degrees/ second 
1 
1 
1 
Qro 1 
] 
. 001 
PuOL 
1 = RoGk 
f2 001 
0.470800 0.118200 -.018969 0.022660 
0, 118200 0.075907 -,043117 0, 012530 
K : SO UUS ENS ES -, 043117 0.501400 0.136100 
—f2 0.022660 0.012530 0.136100 0.099234 
0.003000 0.002000 -.032000 -. 450000 
0.004000 0.016000 0,000000 0, 000000 
Filter Eigenvalues 
Real Part Imaginary Part 
-, 3287 0 
-, 3287 -, 3760 
= lols - 2268 
-. 1813 -, 2268 
~, 2430 ORC 


-, 07681 O20 


Standard Deviation Pa 


or ah 


pd pede pd pee 


. 001 
. O01 
. O01 
. O01 


ot 





speed: 


Table V K 
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and Kelated Data 


12 knots: Ordered Bubble: 


Input Vector: Uso (equation II-32) 


Measurement Matrix: 


Meehnigue: Potter 


—f3 


—{3 


Variable E(w or v) 


Sb 0 
os 0 
EF Zext 0 
F Mext 0 
Au 0 
Bx @) 
Awe 0 

e 

Z, 0 
Zoe 0 
go 0 
ay 0 
| 1910 

25 

, 04 

0. 38940000 

0, 12340000 

-~, 00018560 

: 0,00091933 
0. 00481060 

L, 9. 02275400 


i 


Cry (equation [[-34) 


1014 


oO 


Pol 


miigabsiéh@ 
. 801800 


-, 020016 


i 


. 021942 
.033007 


Bee viat ont Ors Cea | 


2 degrees 4 
| degree l 
10° pounds fon 
10’ foot pounds i014 
2 feet/second 4 
Q.1 feet aot 
0.05 megapounds . 0025 
Ua Feet 20 
0.2 feet/second . 04 
O.1 degrees .O1 
0.1 degrees/second 01 
nel 
0025 

-, 0046404 0.022983 

-,0800630 -.087767 

0.8350000 QQ, 451200 

0.4512000 0.677100 

-,5456000 -.835100 

0.035506 0.084360 


. 242600 





Table VI K and Related Data 


f4 
Speed: 12 knots; Ordered Bubble: -1l 
Input Vector: Uso (equation I[I-32) 


o 


Measurement Matrix: Coo (equation II35) 


Technigue: Potter 


Wariable E(w or v) Standard Deviation ae Gr ah 
b 0 2 degrees + 
Is 0 1] degree l 10 
FZext 0 10° pounds 10 
FMext 0 10? foot pounds 1014 
Au 0 2 feet/second 4 
Axa, 0 0.1 feet Ol 
AW, 0 0,05 megapounds 0025 
Zoe 0 0.5 feet yes 
ue 0 Umidecr ees . Ol 
Ge 0 0.1 degrees/second 01 
4 
l 
lo ag 
Q 10 
—f{4 4 
od 
unt 
2S 
P = Ol 
—f4 01 
0.9680000 0.0 -,048367 0.082643 
” 0.4687000 0,0 -.132600 -,064191 
K., = -.0019347 0.0 0.836400 0, 451900 
—{4 0.0033057 0.0 0.451900 0.678400 
0,0113010 0.0 -,546800 -,835400 
0.0998360 0.0 0.019525 0.020985 | 


The actual dimension of Key are 6 X 3. The above format is required 

for the computer simulation program (Appendix F), The column of 
Ae | 

zeroes implies that Zoe is not measured and that zis not fed back 


to the filter. 





4, Filter Simulations 

Filter simulation results are grouped by gains matrices, Re- 
sponses to step and ramp (flooding) inputs were simulated, The step 
inputs in Be were plus or minus 11,000 pounds. Table VII illustrates 
the comparison of this error to various types of displacements for 


tine test submarine, Other than noise-free 


Classification of Displacement Displacement (tons) W,/Dis- 
placement 
Hydrodynamic (incl, free flooding) (32 .0746% 
Submerged 7170 0767% 
surfaced 6100 .0902% 


Table VII. Comparison of es step to Displacement 


Simulations to determine the natural responses of the filters, two 
sets of noises were used to provide a common base for comparing 
responses. The statistical properties of these Gaussian disturbances 


appear in tables VIII and Ix. 


External Measurement 
Variable E(w) Standard Variable E(v) Standard 
Deviation Deviation 


FZext 1,000 Ib 5 ft 
FMext 10,000 ft-lb; _5 ft/sec 


FXext 500 lb i 
i ysec 
Jl 


wil 
_06 ft/sec 





‘Table VIII, Disturbance Statistics 
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fe xternal Measurement 
Variable E(w) Sesalenetal Variable E(v) Standard 
Deviation Deviation 
FZext 1 lb .0316 ft 


EF Mext 1 ft-lb ; .0316 ft/sec 


F Xext ~ Ib SOSIG 
.0316 /sec 


. 0113 ft/sec 





apie ix. Disturbance Statistics 


Figure 10 and 11 illustrate the natural responses of filters 


(Table III) and Keo (Table IV) respectively. It 


Ky is much slower than Keo. The 


disturbances of Table VIII exceed those for which Keo was designed for 


equipped with Key 


can be seen that the response of K 


the most part, but are mostly less than those for which Kp, was 
designed. Comparing the responses (figures 10, 12, and 13) it can be 
seen that the slower filter (Key ) is more stable. Note also, that even 
though the faster filter (Keo is underdesigned, the means of the estimates 
converge toward the actual trim errors. This suggests some sort of 
smoothing might be used to salvage the output of the filter. The rel- 
atively large overshoots of ne for the slower filter (Key) (figure 10) 
can be attributed to lack of damping. This is also reflected in the 
eigenvalues (Table III) for Key . The imaginary parts of one pair are 
nearly twice as large as the real parts. 

The disturbances of Table IX were used for subsequent simulations. 


They are the maximum distrubances for which K,, was SSI and this 


~£2 
filter's response appears in figure 14. es coupling between Ge and 
W , can also be seen, 'Undershoots'' in We lag overshoots of oo by 


about ten seconds. 


Figures 15 thru 18 were plotted for a filter with Kee: An attempt 
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to plot the response of Key to the same conditions (step input) 

revealed that the differences between the two filters could not be 
discerned with the scales used for the graphs, The response of these 
filters (figure 16) is much faster than the previous two filters. In 

fact, the new filters are faster than the submarine ina sense. At 30 
and 60 seconds, depth change commands were given, causing the planes 
to deflect. Before the submarine could respond to the planes, the filter 
"assumed'' the plane deflections were due to steps in trim error. Hence, 
the apparent disturbances of the noiseless simulations at those times. 
This might also explain the fluctuations for the filter with Kyo prior 

to settling out (figure 11). 

When subjected to the disturbances of Table IX, the two new filters 
suffered from relatively severe fluctuations (figure 15) which were 
smoothed somewhat using a least-squares smoother (figure 16), Data 
from the previous thirty seconds was used for smoothing estimates. 
The noise was less than that for which the filters were designed. The 
smoothed estimate of Xa 6 lags the unsmoothed estimate by about six 
seconds while that for ee lags the unsmoothed estimate by only one 
second, This might pe attributed to the fact that the response of MW 
is faster than that of Xa (see figure 16), 

A filter equipped with Kee was subjected to a flooding casualty 
(figures 17 and 18) of 500 pounds/second, from 5 to 60 seconds, at 125 
feet aft of the body axes' reference, The simulation was begun at -30 
seconds with zero estimate errors to ensure the filter had "settled out" 
prior to the casualty. The smoothed estimate of XA 6 lags the beginning 
of the casualty by about 12 seconds and gets within 5 seconds, while 
that of Ne lags by 5 seconds at first and getS within 3 seconds, ex- 


cluding the pertubation at 35 seconds due to the plane deflections, 
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1, Conclusions 

It has been demonstrated that trim analysis lends itself to optimal 
estimation techniques which manipulate measurements from instru- 
ments installed on most U. S. Navy submarines. Sufficient measure- 
ments are those of depth, pitch, and pitch rate. Installation of a rate 
gyro should be a simple task if no instrument, capable of measuring 
pitch rate, is already installed on a given submarine. Although not 
required, measurements of rate of descent are useful if available. A 
use by-product of a Wiener filter designed for this task is its ability 
to estimate the severity of a flooding casualty. This information could 
be of benefit to an officer of the deck by alerting him of the casualty 
and by providing him with information necessary for determining the 
extent of recovery action required. These estimates can be made while 
conducting maneuvers in the vertical plane. 

A potential problem area exists in the variation of the physical 
system from the linearized filter model. The simulations described 
herein are ‘first cut" in that the filter model coincides with the true 
system model. Inreality, there are two sources of error here. First, 
the coefficients of motion might not be accurate. Secondly, as devia- 
tions from the equilibrium conditions become larger, the accuracy of 
the linearized equations of the filter is degraded. Motion in the hori- 
zontal plane is another deviation for which the filter is not equipped. 

It can be expected that modeling errors will be reflected by the filter 
aS measurement and estimate errors. Filter instability is another 
possibility. Being specific about differences in coefficients, some of these 
variations might be predictable and lend themselves to updating by a 
computer. Concerning deviations from equilibrium conditions, the 
control surfaces are probably the worst offenders. However, they are 
treated as inputs by the filter. Hence, they need not be linear, and it 
would be satisfactory if forces and moments were stored in a computer 
as functions of plane deflection. Filter gains would remain the same. 
The moment of inertia can also change appreciably, depending on how 
water is distributed in the trim system. This phenomenon is some- 


times utilized by submariners to ‘tune’ the resonant frequency of the 





submarine in pitch near the surface. It might be advisable, therefore, 
to feedback water levels in the trim tanks to the filter. 

Another potential problem area is that of disturbances. The filter 
design is based upon the assumption that they are stationary, normally 
distributed random variables with known statistical properties. While 
it might well be possible to approach this situation for the measure- 
ment distrubances, it is quite another matter for external disturbances, 
Although no problem exists while the submarine is deep, near surface 
effects can be very dramatic. In fact, the submarine's response might 
be very ‘broad band" compared with typical sea spectrums. It is 
under such circumstances that the smoother becomes valuable. The 
best period over which the smoothing should be done might well be the 
subject of another thesis. However, if the trim error changes very 
slowly, which it usually does, the luxury of longer periods could be used 
to obtain smoother estimates. 

mnere are other sources of error which have been neglected such 
as hull compressibility effects and free surface effects of fluids in tanks 
or the bilges. It is felt that if not already capable, the filter can be 
suited to such problems by imaginative uses of the covariance matrices 
to reflect uncertainties, 

Concerning filter gains, susceptibility to noise varies inversely 
with filter response time. For a true Kalman filter, the gains are 
functions of time so that initial errors are heavily weighted to ensure 
rapid convergence followed by relatively smooth tracking. While this 
is good for variables with large variations, it is not felt to be necessary 
for trim error variables. This might not be true if the filter is intended 
to detect and measure flooding casualties. Since these objectives are 
contradictory, it might be well to consider two Separate filters rather than 
one which does neither job well. 

It is not expected that real time applications would be difficult, 
given the present state-of-the-art of computer technology. Despite 
the Author's usage of the relatively slow fourth order Runge-Kutta 


technique, complete problem set-ups and simulations took about half of 
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the real time simulated on the IBM 360 computer. 


2. Recommendations 

Before the actual application of such a filter to atest platform, 
the following factors should be considered: 

The filter must be manipulated to be as sophisticated as is useful 
for the given hardware. This means considering such factors as 
making the filter discrete or continuous, Shall the gains be calculated 
on the submarine's computer, or precalculated and stored therein? 
Should the system matrices be precalculated or stored? Before making 
such decisions, an extensive sensitivity analysis should be conducted to 
ascertain the effect of previously described uncertainties on the filter's 
performance. Most challenging would be the study and simulation of 
near surface effects. 

Reference (7) is packed with practical considerations and, inthe 
opinion of the Author, could be of immense value in any such under- 
taking. Contained therein are many examples of and inovative solutions 


to such problems. 
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Appendix A 


Submarine Equations 
of Motion in 


the Vertical Plane 
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fee ceneral 

The first step inthe solution of the control and estimation 
problems is the derivation and linearization of the equations of 
motion of the submarine. In order to restrain the magnitude of the 
problem, this derivation is limited to the three degrees of freedom 
in the vertical plane. These are pitch, heave, and surge. Although 
this constrains the applicability of the results somewhat, the constraint 
should not be too severe for two reasons. First, submarines spend the 
vast majority of their underway time traveling on a straight course 
between the proverbial points A and B. Secondly, for most course 
changes, it is expected that the coupling between the vertical and hori- 
zontal planes will not be so severe as to render the trim filter useless. 
In fact, most of the coupling is through second order terms which would 
be dropped in the linearization process herein. In short, it is not felt 
that the slight increase in accuracy during relatively rare manuevers 
justifies the added complexity brought upon by the added three degrees 
of freedom in the horizontal plane. - 


The basic forms of the equations are: 


r= d/at (im U.) (A-1) 


and 
a Ad /dt im (A-2) 


(A-1) simply says that the rate of change of momentum of a body 
relative to an inertial reference is equal to the resultant of the applied 
forces. Likewise, equation (A-2) implies that the rate of change of the 
angular momentum of a body is equal tothe applied torgue. Figure l 
On page xx illustrates the axes systems. The a and Z , axes are 
fixed relative to the earth and are directed horizontally and vertically 
downward respectively. The earth will be assumed to be an inertial 


Peterence. Such an assumption will cause negligible errors even for 





those relatively high speeds attained by submarines returning from 
extended deployments. 

The x and z axes are the body axes of the submarine. Their 
origin is the point assumed to be the body axes origin during the deri- 
vation of the coefficients of motion. However, due to the changes in 
the position of the center of gravity relative to the submarine for 
various trim conditions, the origin of the body axes and the C. G. will 
rarely cancide. Furthermore, the x-z plane is alsoa plane of 
geometric symmetry of the submarine. This is also assumed to be 
meorane Of mass symmetry. Hence, the y axis which is perpendicular 


to the x- z plane, is assumed to be a principal axis of inertia. 


2. Inertial Forces and Moments 

Using the techniques described in reference (1), the right hand 
sides of the basic equations can be broken down into the various com- 
ponents of the inertial reactions relative to the submarine's axes origin. 
(Wherever possible, the standardized nomenclature of reference (2) 


has been utilized.) Starting with equation (A-1), 


d/dt (mU.)= mUe + mU, (A-3) 
Normally, rates of change of the mass and the center of gravity of 
the vehicle are considered negligible for ane applications and are 
omitted. In this case, they shall be retained for the sake of generality 
during trimming or flooding of the submarine. 


The velocity of the CG becomes 
Ue=U ao tae he (A-4) 
where 


”~ JIN 
U =uitwk (A-5) 


nd 





aan 5 A” ra 
J = Ue 6 T Weel K (A-6) 


()= s J (AT) 


A ~~ 
7 oe (A-8 
aan Xe b ZEN | ) 
Hence, 
A NX 


f1XR.= Ze4 ea xe GK (A-9) 


After appropriate substitutions and grouping of terms, equation 


(A-4) becomes 


A A 
Cs Cu af psa a 264) ut Cw Ne Xe) k ‘A-10) 


Next, the acceleration of the CG must be described. 
Ue=U + Ure FLX RAOKOXR, (A-11) 
| i Ae | 
ec @) * ret 


It can be seen in equation (A-11) that there are five components 
Meee acceleration of the CG. The first is that due to the acceleration 
of the body axes relative to the inertial axes. The second is that 
due to the translational acceleration of the CG relative to the body axes. 
The third is due to the angular acceleration of the body axes and the 


fourth is the centripetal acceleration due to the body axis rotation. 
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The last contribution is that of the Coriolis acceleration. It is now 


necessary to reduce the components of |) e jini her. 


——— 


Differentiating equation (A-5) with respect to time, 


MM eK tu di/dt+wdk/dt a 


The first two terms on the right hand side of equation (A-12) 
represent the translational acceleration of the body axes. The second 
two terms reflect the change in the orientation of the unit vectors due 


to the rotation of the body axes, ie., 


ae / ct =AX7=- ak (A-13) 
and 7 

dk/ dt =QXk= gi (A-14) 
Hence, equation (A-i2) becomes 


U= (utwa)i + (w-ug) k (A-15) 


Also, 
B A ~ 
Bel ~ Urey & * Were! k (A-16) 


() = g j (A-17) 


mex R. ZeGit x.qk (A-18) 
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Using equation (A-9) 

A A» 
mex s) xX R= Xe qt Zeq"k (A-19) 
Finally, 


2 ag eee ae Weel | a = Ure K (eee) 


Substituting equations (A-15, 16, 18, 19, and 20) into equation (A-1)1) 


and grouping terms yields, 


e ; ; A 
eee Ko 2 qa) L - 
(A-2] 


+ wo ug + Wre 7 Xe. Ze 2.4 Ure) k 


Next equations (A-10 and 21) are substituted into equation (A-3), yielding 


~d/dat (mU,) eal = Zk (A-22) 


where the surge component of the rate of change in linear momentum is 
= + = i ‘- 
Xe mm CU Ure ZO) (A-23) 


+m(u v wot U rel FZ eG xq" + 20 ret) 


and the heave component is 


i: wt Weer xo} (A-24) 


ae (Wr UuatWy7 XG ~ Zeq 2.4 Ure} 
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It is necessary to rewrite both sides of equation (A-2) at this 


stage. Expressing torque about the body axis, 


Mee | ~ RX E (A-25) 


where 


~ 


On the right hand side of equation (A-2) is 
me 1, £2) (A-27) 


Differentiating equation (A-27) with respect to time, 


d/dt H.= 1.0 “LQ (A-28) 


Or 


d/dt H= (Le, 4 zi 1.4) j (A-29) 


where 

acy oy 7m Car z2 ) (A- 30) 
and 

I.,= 1, - ra (x2 + 22) (A-31) 


—™ ™M (2 Xe Urel y 2 Zeer} 


Somibinine(A-c5, 26, 29, 30, and 31 ) and rearranging terms 
yields 





ae (z.X,- he 4 | gmx, ze) *q 1, 
nN (A-32) 


re i 2 ZeWrel) 4 ] 


~am( xe Zale qm (2X, 


Also 


a. } | (A-34) 


several cancellations occur when equations (A-23 and 24) are 
substituted into equation (A-32). Regrouping terms and equating the 


results to equation (A-34) yields 


Mes Lt gl tmlzglutul—xlw twee 14-351 


S 


+ m| z,\ut wa Ue} xglwougt Wret) J 


This completes the modification of the right hand sides of equations 


(A-l and 2). Now the left hand sides must be more adequately described. 


3. Applied Forces and Moments 

Brera rence (3) gives a comprehensive breakdown of the applied 
forces and moments. The coefficients are partial derivatives of the 
hydrodynamic force or moment with respect to the subscripted variable. 


For example, 


MI i SM / dw 





For the vertical plane, the surge cornponent of the applied forces is: 


= BS. . or +X, U *X a4 ae ue + XW 
+A. UG 43 ye UU. Gi 8 é > (WB) sin OS 


5 (A-36) 
* Xa? RN N+ (u2/UVLX 6, St an 
X seen 5 Ss (4-| 1) +> Disturbances (surge) 
The heave component of oe applied forces is: 
ma = : q Wy aren Iq | Lote WD 1 eh 
a —-B) cos 6 + Pare: w |wl (n—-|) +(u/U) ee a ee 


i... al d+ yal Vee Iwi+1Z, 4 * Zn) 
-(R=I)} + lu? /U ) wea an eae d¢ 
o.., Se | ) | + > Disturbances (heave) 


Iw 
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For the vertical plane, the only component of the applied torque 


is the pitch component. The applied torque in pitch is: 


Me= Mag * Maiq 4! se Naas Mig WI 


+M ow iwl + Mow? -(x,W>- x, B) cos © 
-(z,W- zB) sin® + Manag l(t) 
+(u/U))M, o + Marge al $3 + Mow 
+*M Iw +LM,, q* M on w | (n-1)$ (A-38) 
+(2/U) [Mt Mea Sot Me Se 

Bcc, 2s (r-I)J+ = Disturbances (pitch) 


4, Change of Variables 
Generally, the motions of a submarine in the vertical plane are 


described in terms of ahead speed (u), pitch (9), and depth ( with 


Z, 
O 
the origin at the sea surface). These are illustrated in Figure l 

On page xx . This is due in part to the measurements which are 
available. The preceeding equations of motion must be modified so 


as to use the available measurements directly. The required change 


of variables is accomplished using the following equations: 
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Zz =w cos O —u sin® (A-39) 


oO 


ma cosas — AS sing 
(A-40) 
mci oo Ug cos 
Furthermore, for the control and estimation aspects of this thesis 
it is useful to express the variables in terms of command depth 
(commonly referred to as ordered depth), Z 3 deviation from ordered 
depth, Zo! command pitch (ordered bubble), 2 and deviation from 


ordered bubble, o . 


7 = 7 +g (A-41) 


and 


= Se as Ss) (yeaZ) 


Since oe. and Zoe are piecewise constant, 
> =F (A-43) 


7 -Z (A-44) 


= = (A-45) 


<i €-) (A-46) 
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The following additional transformations result: 
=| 
= = +- } 
2... | cos (9. me 6) | u tan (Oo. i 9) (A-47) 


, and 
w= Z..| cos ( Q.+ oni + ou OG. 
+ (w Qe, + u) tan(Q.+ 8.) 


(A-48) 


5, Linearization 
The application of optimal control and estimation theory is well 
served if the equations of motion are linearized. This is accomplished 
using the techniques described in reference (4). Basically, a Taylor's 
series expansion of all variables is made about an equilibrium condi- 
tion. After substituting these expansions into the equations of motion, 
terms of higher than first order are omitted. For small perturbations 
about the equilibrium condition, the actual equations are very closely 
approximated by the linearized equations. As the magnitude of the pertu- 
bations increases, the accuracy of the linearized equations is degraded. 
The equilibrium condition used will be for the submarine traveling 
horizontally at a steady speed and with a steady pitch angle. The 
object of this thesis is to determine the magnitude of pertubations 
Sreume Submarine's trim from the "in trim" condition. Bieter ore 
as part of the equilibrium condition it is necessary to include the ‘in 
trim" state of weight and longitudinal center of gravity. More 
details on the equilibrium conditions appear in Appendix B. The 


meearized variables are as follows: 


sin O= =. COS S. + sing (A-49) 
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cos © ees =, = Sr sin S. (A-50) 


Practically speaking, unless the submarine is grossly out of 
trim, the magnitude of the ordered bubble for depth keeping purposes 
is always less than seven degrees. Therefore, the second term in 
equation (A-50) is of the same order of magnitude as a second order 


term and can be dropped, leaving 


Beso = cos. Peo) 


Other linearized variables are 


tanQ@= 0. + tan = (A-52) 
See. + Au (A-53) 
oA u (A-54) 
ero OU (A-55) 
W a W Bs We (im . a\net me] awe 
W= A VV) Views 


m= | +al (A-58) 


m= Al (A-59) 


— - xX x ky, 





- 


a 


mes 
il 


Linearized products of certain variables are: 
Mecano — u,O,*+ u,tan SE eA oh 
Using equation (A-63) to linearize equation (A-47) yields 


: =| 
vy maces © | US. inten aie 


a YC) Ee auaen 


which can be used in the linearization of equation (A- 48): 


we z,.(cos@) + uy (l+tan*@) 6. 


muy seein toh 


From equation (A-64) it can be seen that 


Pe u. tang. 


Oo 


and 


: = 
Aw = 2.. (cos @.) + u,O8, + au tan 9, 


oe 
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(A-61) 


(A-62) 


(A-63) 


(A-64) 


(A-65) 


(A-66) 


(A-67) 
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These variables are used to more clearly illustrate the following 


linearized variables: 


lw} = u, |tan Ol 
—_ 


2 
0 ll Zw, AW 


w lw] = welw.) + Zlw,) aw 
Also, 


lq ~ 18, 

wa =u, tan®&, &, 
Me Se AAU 
S= AS, 

eee AN SS. 


a — Sat 


Bo —- cos +t AX sind. 


(A-68) 


(A-69) 


(A-70) 


(A-~71) 


(A-72) 


(A-73) 


(A-74) 


(A-~75) 


(A-76) 


(A-77) 


Once again, since © is expected to be less than seven degrees, the 
e 


second term of equation (A-77) is of the same order of magnitude 


as a second order term. Therefore it is dropped, leaving 
Sos = cos ©. 


al 


For ahead motion only 


(A-78) 





Since 6, a u/ cos oS 
the linearized version of velocity becomes 


ou, + Au)/ cos oO. (A-79) 


Hence, 


u/U = |/cos ©. : (A-80) 


For the axial propeller thrust equations 


M=u.7 U (A-81) 


where 
a = CVn oa 6 (A-82) 


for variable rpm simulations. 


Linearizing equation (A-82) 


feet 7, + au. /U,- u,. Neo (A-83) 


Substituting the components of equation (A-79) in equation (A-83) yields 


= Sec Os S. oe mes Els SCs = Ja. (A-84) 


pre 2. 
Uypey e motor 6 /u 


Therefore, 





1S 


(n-\} = coe cos O. /u,) = 


+(cos Sy /u.) VES MO Cus. beet. /ud) AW (Aa) 


For the sake of the simulations herein, rpm is considered to remain 
constant. This does not affect the trim filter in that it does not attempt 
to estimate changes in surge. The filter merely uses the measure- 


ments of Au. This policy simplifies equation (A-85): 


(n-1) = Pee Sy AU (A-86) 


where 


om 7 Cu cos So. / Uo) cle (A-87) 


and 


=) 7a. 
eee .. cos Oz / (A~88) 
This completes the bulk of the linearization of the variables with 


the exceptions of w al se) , and z_. For the linearized equations. 
rel rel rel G 


it shall be assumed that changes in the height of the CG are extremely 
small for all practical cases. Hence, the effect on the dynamics of 
the submarine as predicted by the linearized equations would be negli- 


gible. From the assumption that 


eZ. = O (A-89) 
metollows also that 


el W rel =O (A90) 
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for the linearized equations. 
In the longitudinal direction, there exists the possibilities 
of much larger moment arms for flooding and trimming water. Hence 
Ax. is certainly a variable to reckon with. The rate of change of 
Ax, (Uap can be expected to be small for typical rates of flooding 
or trimming. Due to coupling with other variables, Ul does not 
emerge from the linearization process as a first order term, so one 


does not have to make a practical decision as to whether or not it 


should be retained in the linearized equations. This is not the case 


foru .; so the assumption is made, in light of the fact that u is so 
rel rel 

small, that 

: a A-91 

eee, UO (A-91) 


for the linearized equations. 
Substituting the linearized variables into equations (A-23) and (A-36) 


and arranging terms yields the linearized surge equation: 


a, = c. = + (m7 x.) Got (u, /g) W 
=(X., /cos 8.) Za on ee = a _—_ S. 
BP xu AU sin oO. W a ie ‘i Pasi Saat 


where 


Uo Ke — OWE B) Goer SS, (A-93) 
B.., we ee Orr Cy 5 akin 
ee Clete at ea (A-95) 


W 


o 
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(A-96) 


X au 72 ulA, ‘5 X va) - Bi oe 


2. (A-97) 
+Crho Xin We + Xa, tan &. 


© 


xs 7 Ws ( a Cy Kon) os oF C¥e 
+ u.LuCA.+ X ua) t Bea, | —(W.-B} ae (A-98) 


and Cw) and G are defined by equations (A-87} and (A-88). 


eZ 
Equations (A-24) and (A-37) lead to the linearized heave equation: 


Tae ~ cane =e = ak 2B ekg) = U 
+ (w, /q) W = ae Veen 6, | Za + aa oe S. 
+Z nse Cnt co*®O.Z,, Stas, Se (4-09) 


manne cos OW + Et Fe 


where 


Ta de / cos So: (A-100) 


Azoe 


Tle >. Sea —/. cea konly 


AW oO WA 





DSc ce. s va 


\ 


(A- 102) 


ye ., = ea lw, | Zone Sin) +r PM. 


COS oe CZ. 1 ae 


mae WwW. Cy vane lw, a eee, » 


i VS tan = 


ex... i. oa + ates CO. 


(A-103) 


(A-104) 


(A-105) 


wig (\n/— = Be ae Ia | + cas OQ. LOS =e 


—— 


tw | | Wa | (Z orer* Cr Z vsrenn) 
Wo Zo (Zu, * Cor Zovun) COS e._| 


Boke’ 


eee eas) cin. 
+ usLmn— Zw (1 * tan? &) 


W | , Ww Cee, awd. © Se rac NuCaymnecduatiods: 
O 


Sel a MZ 
Mmeos), (A-95), (A-87), and (A-88) respectively. 





Finally, equations (A-35) and (A-38) lead to the lin 


equation: 


(A-106) 


(A-J07)j 


Merri zed piven 





i 


(Mas, /cos @) 2, + (T.-M) O. +Maa & 
+ Mang W = (Mw (cos G.) 2, See, (aS 08) 


See € 


+M,2. © Meme cose. ey | lac. SoM, AU 


aee 


a W, CSS), Kes S Mame We i Fie “ | iat 


where 


Mo=- Xe. Me - Mo (A-109) 


I 


Maw = 2lwol( Mii * Car Mirnwied £2 May W 


w iv 
A-110 
[My aj Cry a) SESE . | 
May = Zemet May tan & aca 


(A-1li2) 


Marg oa (zo u.- Xoo) / 9 
Bsa Ue es WW rales B) cos OL (A-113) 
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ze ( 
Pl ae oo NGs Us ZeWo} a M ete We | (A-114) 


a oe (| Teele e.) + lat Cr Man) Cone 


os 
PAs. eM. a Cu Pe) COS =a (AS115) 
Ae = ee eens 
(A-116) 
en ~~ Keg COS = ENG, = (A-117) 


i 


Ms 


7a N= eas B) sin &. + Wal lw ee 
+Cry Mon) * We Mut (Mit Ca, Mun) cos Gc] 
+(y iS a a Nae al lw, Mi ae COs ©.) (A-118) 


Ese Se 


and |W 
O 





Cpe pa m2 
(A-87), and (A-88) respectively. 


me Ge, and 1G are defined in equations (A-68), (A-95), 
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Equilibrium Conditions 


and Deviations Therefrom 





80 


l General 

In the linearization of the euqations of motion, equilibrium 
conditions were referred to. These are the values about which vari- 
ables are expanded in the linearization process. The manner of and 
the logic behind the choice of these conditions is discussed in this 
appendix. It should be understood that, unless stated otherwise herein, 
the equilibrium value of any variable is zero. It can be seen that all 


conditions evolve from the choice of ordered bubble and shaft rpm. 


Pee auilibrium Speeds 

In the actual operation of a submarine, equilibrium speeds during 
level flight are functions of equilibrium pitch and shaft rpm. For 
the computer simulations, shaft rpm was considered to remain constant 
to avoid the ambiguities which would have arisen in attempting to model 
the dynamics of the propulsion plant. The filter gains are predicated 
upon a value of ue. which is a function of shaft rpm. In the actual im- 
plementation of the filter, a provision for the input of shaft rpm should 
be sufficient to keep the filter updated for changes in ua 

The evaluation of ee is necessarily one of the first steps inthe 
Simulations. The coefficients for the equations of motion are in di- 
Meensi0nal form. To calculate them, it is necessary to know the 


value of U . 
O 


The equilibrium speed problem is stated thusly: 
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Given: u and9O 
c c 


Find: Ma eee cad. UL) 
O O O 





Figure b-l. Representation of 


Equilibrium Speeds 


Figure b-] illustrates the relationships between most of the 


variables. They are: 
we — u, / cos = (ae 


and 


WwW, U, tan & (B-2) 


The solutionof the speeds is begun with that of uo. This is done 
via the linearized surge equation, equation (A-92). In the equilibrium 
condition, assuming negligible external disturbances in surge, equation 


(A-~92) reduces to 


a --.. (Bas) 
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which can be rearranged to read as follows: 
a 2 
O= (Xt Ay) ut + Buu, tC; uz 


a. ae (Y,- |) X wel we ON B) sin. (B-4) 


where 
No=u./U, = u, cos © /u, (B-5) 


2 
Equation (B-4) is non-dimensionalized by dividing by 1/2 pk : 


Ole (XU, + a.) Ua) Salers Caer Ol 
+L Xe #1) Koved w2 
-(W,- B) sin@. /& pt 


The solution of the "'intrim weight (Ww) follows (equation B-14). As 


(B-6) 


is illustrated in figure 6 , oo is a function of velocity squared. 
Capitalizing upon this and the linear relationships between the velo- 
cities, it is possible to turn equation (B-6) into something resembling 


a quadratic equation for u 
2 a " ie (Z | = | Vie 
U, uu Qi {w) cdg a ‘ sersua 5 
/ 7 / 
+L (Zona * Cor Zweig) |tane.| +Zi tan & 
/ dj (B-7) 
a OY ae Cry Paes eat e.} a Pe Uaule 


Pci. ~ ee tane./+ peo =O 
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where Cy, is defined by equation (A-87). Due to Cy, , equation 
(B-7) is not actually quadratic, but it does lend itself to a iterative 
solution. Fortuitously, for the coefficients used in the numerical 


example herein, equation (B-7) actually is a quadratic equation: 


A’ ie + B‘u, +C =O (B-8) 


where 


A= Xi, ta,+ (Zo + ZZ tane.+XZ, jtan*e@ 


+(Z'. tan©.+ Z.,, tan?) |tan@.| 3-9 


Iw 


+Z tan. 


7 
Pe bu. (B-10) 


and 
C= ¢,u2 + E(F,..) tan @./+ pe ; (B-11) 


There are two solutions to equation (B-8), only one of which is real- 


mstic. Ihis turns out to be 


x —-B’°-J/Bl-4NC (B-12) 
2A 


U. 





for the coefficients in the numerical example. 
Having solved for uo equations (B-1) and (B-2) are used to 


determine ee and w 


pee it) Prim’! Condition 


The equilibrium trim problem can be stated as follows: 


Given: u_ and®© 
@ Cc 
Find: W andx 
O Go 
This is accomplished via the linearized heave and pitch equations, 


equations (A-99) and (A-108) respectively. In equilibrium, the heave 


equation simplifies to 


oot = Pr ue! ae 


This equation is manipulated to find W 


mers Z| Walia: 2 hcos @ — B= oe) /COSEe 


a. ae a oo 4 a oes (B-14) 
a Va 7. e.. al) eos = Ww, / Cos =) 


It should be noted that the bouyant force , B, is assumed to remain 
constant and is equal to the weight of the water displaced by the volume 
of the entire submarine, including the free flooding volume. Therefore, 


all changes in the trim of the submarine will be due to changes in the 
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density of the fluid in which the submarine is submerged, or due 
to changes of the mass within the envelope formed by the outer skin 
of the submarine. 
Having determined W the linearized pitch equation is used 
to determine the longitudinal position of the CG for the "in trim’ 


condition. In equilibrium, equation (A-108) simplifies to 


O = Fine ak oe alia 


Rearranging this equation to solve for Xeno? 
O 


ix,BeM,, Iw, | + Mimics Ss 
(B-16) 
m2 VV = i B)-+tan 7 = ECF oe} (cose 
PaS- IW | CM oat er eee a Wo aes 


(M+ Ca Mug) cose] 


4. Deviations from “in Trim Condition 


The following equations are repeated for clarity. 


W=W,. + W (A-56) 


Miiewaieans of calculating ee and Xa are equations (B-14) and 


(B-16). For the simulations, the submarine was put in an '‘'out of trim" 
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state by adding weights at various longitudinal locations. 


Hence, 
ate added 
Wo = W. (B-17) 
= Anas L 
(=| 
and 


| added 
Xee “Wo ve NN (B-18) 


Gait cE 


The purpose of the trim filter is to make a quantitative estimate 


of these variables. 


5. Changes in Trim while Flooding & Pumping 

Most of the simulations were conducted for step inputs in trim 
errors. A more realistic case is that of flooding or pumping. The 
following equations describe the effect of flooding at a longitudinal 


location designated Xp The rate of change of weight error is 


W = \A/ = We (Be19) 


At any instant, the CG is located at 


Keane W, aye X¢ We 


X — ane 


GC 
VV eee We 


(B-20) 


Differentiating equation (B-20) with respect to time yields 


Ure, = Wes (xem x6) /W (B-21) 





where 
i 
W = Wing +) We dt 
1 
and 
= 
ae Ge int . el At 


Pumping is simulated by making W., less than zero. 


f 
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(eZ) 


(B-23) 
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The general problem of the controller design was stated in section 3 


of chapter II. The linear system is described by 


d = + 
fat x, = AL x, + BL wy (C-1) 


and the purpose of the controller is to minimize the performance index 


oa 


T 
PI = \ ce OG) ee ee ene ee (E=2) 
es ace ee Cres Oeics Ge 


where both e and P. are symmetric, Q. is positive semi-definite, 
and = is positive definite. Physically this means that not all state 
variables need be controlled and that all control variables must be 
weighted to preclude an infinite control effort. The optimal control is the 


feedback of the state: 


2, eee (C-3) 
where 
=a) 
as 
K =P BR (C-4) 
=—c -c -c = 


and R is the solution to the matrix Riccati eetecion (Appendix E). 
Concerning the weighting matrices, it is not their absolute magni- 
tudes that matter so much as their relative magnitude which determines 
the magnitude of the control effort which is proportional to Q. and 
inversely proportional to P. . Also, the relative magnitude of the 
individual elements within each matrix determines the relative importance 
of each of the variables. When the steady state solution of the Riccati 
equation is used to calculate optimal feedback gains, the controller is 


classified as a linear regulator. 
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Appendix D 


Filter Design 


a 
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i. Optimal Filter 
The filter problem was set forth in section 4 of chapter II. The 


filter equation is 


bE K 5 
= aF - = 
ee ep Ae een ee’ See 
where 
a 
K, ~ R Cc P. (D-2) 


and Ris the solution to the matrix Riccati equation (Appendix E) which 
minimizes the performance index 


a ie 


Pr= | (xp Bp Qe Brxy + 


T T 
p¥e + Up Pe upddt (2) 


Reference (7) gives excellent descriptions of the physical significance 
of the solution. The weighting matrices for the filter problem are not 


sO arbitrary as those for the controller problem. For example, Q 


i 
is the covariance matrix of the random system disturbances, w. 
Ab 
Q; = E(ww ) (D- 4) 
For atwo dimensional matrix : 
ec 
E(w, ) E(w w,) 
ae 2 ae 


E(wyw,) E(w 


which illustrates the diagonal elements of Q, are the mean square 
values of the distrubances and the off-diagonal elements are indications 
Seime cross correlation between the elements of w. 


Similarly, for the measurement noise, 


= E(vv } (D-6) 
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Physically, the solution of the Riccati equation is the error 


Sovariance matrix for the filter, 


R= ERE) la 


The Riccati equation for the filter problem is 


dt 


R= A,R + RA -RC) PCR + BQB- 


i. (D-8) 


The first two terms onthe right hand side of equation (D-8) 
result from the unforced system characteristics in the absense of 
measurements. i. Q,.B- accounts for the increase in uncertainty 
due to system noise and - R C; PO C.R R accounts for the decrease in 
uncertainty due to measurements and their quality. 

Looking back at equation (D-2) it can be seen that the filter feedback 
gains are proportional to the uncertainty in the estimate and inversely 
proportional to the measurement noise. 

One other note: when the steady state solution to the Riccati 


equation is used to determine filter gains, the filter is classified asa 


Wiener filter. 


2. woub-Optimal Filter 
When the Riccati equation cannot be solved, some other means of 
determining filter feedback gains must be used. Looking at the filter 


equation in the absense of noise, 


d/dt 5 = Ax + (D-9) 


lt 


u- KC? 


y 


(D-10) 


[XR 
1 
[> 

| 


and 


1s the estimate error. 
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The system equation is 


Wee x Ax + Bu (D-11) 


So if equation (D-11l) is subtracted from (D-9), the result is 


d/dt x = ric Gyr x 
fat x = (A - KO) x (D-12) 


which shows that the filter will be stable provided * goes to zero as 
time increases. Stated in another manner, the filter will be stable, 


provided the roots of the filter's characteristic equation 


det (AIL -A + KC) = 0 (D-13) 


have negative real parts. 

As mentioned earlier, it was necessary to select only the fifth 
and sixth rows of the gains matrix this way, the other gains having 
been found by Potter's method (Appendix E). It was possible to logically 
determine the signs of the gains based upon the qualitative behavior of 
the filter as described by equation (D-9). For example, if ze and/or 


Ze were less than estimated, implying 


Zz > O and/or Zant 4 
oe oe = 


it would imply that the weight error (W ) was less than previously 


estimated. To correct this error, it would be necessary 
decrease W , so 


wt : 2G A iO 
W ee Key an 62 


Due to the coupling between pitch and heave, the same phenomenon 


might be due to pitch being greater than estimated which inturn was 
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due to Xoo being further aft than estimated, and 


: Be 
x,t if 7 o and ki 7 0 


Now if = and/or oe were less than estimated, ie., 


SSX and/or 2S) 2 6 
e 


it might be due to Xoo being further forward than estimated and 


if k..< < 
ete it k= 0 and kk, ,< 0 
Nothing conclusive can be said about W in this case unless Xoo is 
known, so both Ke. and KA were set equal to zero. These results 


@re summarized in Table d-l. 





Table d-l. Signs of Unknown Gains 


Using the constraints inthe table, gains were selected at random 
and the eigenvalues of equation (D-13) ore Pec iaced: Then the gains 
were varied individually and in pairs to determine the sensitivity of 
the filter's poles (eigenvalues) to each of the gains. In this manner, a 
sub-optimum was selected. 

It might be added that the decision concerning Ke. and Ke A being 


zero was adhered to after discovering that camparable gains of either 


aman had very little effect on the poles of the filter. 








5, 


oe Smoothing 


The filtered estimates of We and x 2 tended to reflect the noise in 
the system and measurements. 


The estimates were smoothed using 
the least squares algorithm in reference (9). 


The ‘best fit’ curve for a number of points is assumed to have 
tae Lorm 


>< as 091 ea (D-14) 
It is the best curve in that it minimizes the squares of the deviations, 
Cea (init. 1) 15) 
1 uf 1 
, of the data points from the line. The line will pass through the 
centroid of the n data points, 
= n 
=e] 2 
» om x, (D-16) 
1 
and 
1 nN 
SS 2 t. (D-17) 
iat 
The slope of the line will be 
n 
> (x, - x) (t. Te) 
m= ot —_ (D-18) 
n mee ; 
5 + 
1 
ee 


so that equation (D-14) becomes 


x = m(t - t) + x 


and the standard deviation of the data about the line is 


5 Ee 2 ee 8) 
n > (t,-€ 


: 5 Yo. 
—| > (#4 -€)| 
a 


(D-19) 
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This algorithm was implemented ina subroutine named LESOR 
(Appendix F). 

This could be used for the trim analysis technique mentioned in 
chapter I. The plane deflections and pitch angle could be smoothed 
while the submarine stayed in the vicinity of ordered depth. Pre- 
sumably slopes would be zero and the results would merely be the averaged 
values of the angles which could be substituted into the linearized heave 
and pitch equations (equations A-99 and A-108). These would then be 
solved algebraically to determine the state of the trim. The simplified 


equations are 


OS VE Cr or ose O.Z, 4 


(D-20) 
a Va be + cos O. We 


and 


O= line oy soa Cosa —e ve 


Sb 


a 
—— VN Couns eee laws We ae 


Of course, we is based upon knowledge of the mean external disturbance 
in heave (equation B- 14). If this is not known, the value of W for 
no mean heave distrubance could be used without serious effects. Ww. 
could be driven to zero based upon equation (D-20) without knowledge 
of external disturbances. Then equation (D-21) would indicate the sign 
of x and closely approximate its magnitude. The true solution would 


Ge 


be converged upon as the submarine was trimmed. 








Appendix E 


The Matrix Riccati Equation 
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mee =e neral 
Calculation of the feedback gains for the optimal controller and the 
Kalman filter requires the solution of the matrix Riccati equation. 


The equation is the solution to the following problem. 


Given: 6h sts odie) sec (1) at mec as) (E-1) 
Find: u(t) to minimize the quadratic performance index 
Ys 
r Ji 
a \ ee ae (E-2) 
° 


where Q is positive definite and P is at least positive semi-definite. 
bn and P are symmetric. 


Using calculus of variations, the solution is found to be 


u(x, t) = -K (t) x(t) es) 
where 
it) = Se R(t) (E-4) 


and R(t)is the solution to the nonlinear differential equation 


R(t) +Q- R(t)B PB’ R(t) +R(t)A +A Rit) = © (E-5) 


subject to the boundary condition 


R(T) = constant (126) 


iM@udtion (H=5)'ie the matrix Riccati equation. Reference (5) is 
one of many references giving excellent detailed descriptions of the 
derivation of the Riccati equation. Readers so inclined are referred 
to those references. 

The requirement of this thesis is the solution of the Riccati 
equation as t goes to infinity. The principal techniques for the 


solution follow. 





oo 


2. Integration Method 

The most obvious method of solving the matrix Riccati equation is 
by integration of the n simultaneous nonlinear differential equations 
from the boundary condition to a steady state value. 

For the controller problem, the integration is performed backwards 
from the boundary condition, R(eo ) =O. Whereas for the filter 
problem, the integration is performed forward in time from R(O) = 


E [ x(0) x’ (0)] . 


The subroutine named MRS (Appendix F) is capable of this integration. 


3. Eigenvector Decomposition 


This method is commonly referred to as Potter's method. It yields 
the steady state solution of the matrix Riccati equation (reduced Riccati 
equation), provided the coefficient matrices are constant, ie., 

A lea 
Q+RA+A R-RBP BR = Q (E-7) 
The solution is found in terms of the eigenvectors of the 2n X 2n 


partitioned matrix 


aaeelleo 


| 

| — 

M = . 

= = (E-8) 
BP epg” aN 


The eigenvalues and corresponding eigenvectors of M are found: 


(E-9) 


where the subscripts denote the signs of the real parts of the eigenvalues, 


and the corresponding eigenvectors are 
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em 1 4 


eer —T- 
veo = | ——-+-— — (E-10) 
Vau | 3. 


V' is partitioned and the solution to the proHem is 


-l 
erp, Spt alee, 


Reference (5) contains the proof of the method. The eigenvalue/ 
eigenvector decomposition was performed using EISPAC (Appendix 
F) and these results were manipulated by the subroutine named MRA 


(Appendix F). 


4. Controller/Filter Duality 
As has been previously implied, the optimal controller and Kalman 


filter problems are dual problems. Table e-] illustrates the duality. 
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Computer Programs 
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A listing of some of the computer programs used in this thesis 
appear herein. The programming language was FORTRAN and 
comments are interspersed for the reader's benefit. Due to the 
magnitude of the packages, ACCESS II and EFISPAC are not listed. 

ACCESS II is a collection of engineering application programs in 
the MIT Mechanical Engineering/ Civil Engineering joint Computer 
Facility. It is an updated version of an earlier package, and is pri- 
marily the work of Professor Richard S. Sidell, Instructor in Mechanical 
Engineering. ACCESS II is run on an INTERDATA M70 computer. 


The following portions of the package were used for this thesis: 


CONTROL Forms a partitioned matrix like that on 
page 22 and determines the rank of 
the matrix to establish system control- 
lability or observability, 


EIGENVALUES Computes the eigenvalues of a system 
matrix, 
MRA Computes the steady state solution to the 


matrix Riccati equation using Potter's 
method, and 


misPAC Calculates complex eigenvalues and 
eigenvectors of a general real matrix. 
It was recently incorporated with ACCESS 
Ilias a subroutine for MRA. EISPAC 
is taken from FISPACK, an eigensystem 
problem solver package developed through 
a National Science Foundation project 
known as NATS (National Activity to Test 
Software). The subroutines were tested 
principally at Argonne National Lab- 
oratory, the University of Texas, and 
Stanford University. 


The program and associated subroutines for the computer simulations 
is listed herein. Subroutines called but not listed are described 


in reference (10) and are: 


MINV Inverts a square matrix, and 


GAUSS Generates a random, normally 
distributed variable 
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ws 


RKGS Uses the Runge-Kutta method to 
obtain an approximate solution to a 
set of first order ordinary differential 


equations. 


Two other subroutines, MRS and MRDOT, are listed. These 
were taken from ACCESS II and modified for the IBM 360 used by the 
Author. These subroutines calculate the steady state solution of the 


matrix Riccati equation using the integration technique. 


— a a 
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Merete —-INiC TAL TI¥? 
Meet 2y = FINAL TIME 
meets TAL TIME INCREMENT 
RP te) =—2ERRCR BOUNC 
Seer tnt, 1200) (P2¥%5(1),2=1,4),ITSTEP 
ae POO Gem er (Ly) yp ley 3) 
as eee) to 
Meme tO, 2 725) (Oe MT (Ll) ,l=2,4) 
Meera g 1. Y DEP Mowe eee EE RPOR WEEGHLS 
PePpo{(KiI, ee es 
emer ,OODING CASUALTY PFRAMETERS 
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= 
UY 


4 





lil 
MAIN 


i 


Pps 


G2 
rx 
= 

+3 
— 
t-4 
we] 
pe | 
ti 
= 

+3 
( 
3 
© 
ry 


or 


Peon (KI, 1004) FLOR 
TX=3479 

PeoP=0. 

IBEINT=0 
TPriNT=0.0 
ae 

i sweet ee ter kK, |\o,LULF,FCT,O0UT?, AUX) 


meee (719,780, 3) 

a a a) 

Meee A (310. 3) 

F Be rs. 3) 

Je PEAT (6F10, 7) 

se -* eC LO Sa} 

F ~~ a (esea 109 ne) 

3 er i0) 

Mee Ao (2F10.3,2110) 

e ey Gh oer Nee DISTUSEANCE STANDAPO DEVIATIONS:*, 


ahh. 2) 


NS tei ag Sas po a a i oT - ch a 


ae ee ees 


FORMAT (1#0,'“’SP. NOISE SIND. DEVIATIONS:',127F14.6) 

F eeerres 30X, 'CONTFOLLER GAINS") 

FORMAT(1H ,2(1PSL1S.6 eae) 

FOSMAT(1HO,25%,' FILZER GAINS") 

FOPMAT (1H ,o(1P45E15.6,/7,1X)) 

FOEMAT(IH ,5(1PS214.6,/,1%)) 

FOFMAT(1H ,3(1PLF15.6,/,1%)) 

epee (1110, '=2Al TIME=',FE.0,' SEC, TIME STEP=',F6.4, 
Meise b> FRAO RCUND=', F6. 2) 

eee 7 (119,20%,°A MAT2LX (ANGLES IN DEGREES AND TDIM, 
' ERROR IN MEGAP POUNDS AND FEET) ¢) 

ee (120. 30X, FILTER A “ATRIX') 

FOFMAT(1 Freee MATEIX') 

Por aS (1 SY pieg tr w(A1GLES Tho DEGREES) "} 
FOPMAT(110,25X, Whe tee (2NGUES I DRGP EPS) ") 
iio. 25X, FILTES 8 “ATRIX‘) 

PommeAT (1H ,'VEUECLE STEADY STATE FCR8CES:',1P214.6, 
a Paes s 677555) LoS 

Meet (1H ,S(iPSE1N.E,/,1%)) . 

Poe “37 (14 "G(4P3IEIN.6, 7, 1X)) 

ere (1 70,'2D=",53,', TUSNS FOR’, FS.1,' KNCTS,', 
Mecerre=iD 238R72=!,F4,1,' PEGPEFS, DINSITY=',F7.4) 
Meee 47 (110, 'MTAN DISTURBANCES APE:', 1P3E 15.6) 

FORMAT (110, ! DISULACIMENT=", 1PE13.6,! Pcs weeOuT MT WS ts 
MePTSPLACSMINT=', £13.6,' 233, XGO=',215.5,2% F7) 

Pee mar (1HO,' 2b 24 SeROR=',1P013.6,' Las, 1 213.6,' PT. IN LCG") 
Mees (id, VELOCITIES (FI/S2C)> UC=',1P£L12.€,5H, C= 
Mame ee 64, Vol=,215.6, di, kU=,E13.¢) 

STOP 





112 


MAIN 


PeyO POULT NE MPYD(A, B,C,NR,NK,NC) 
Bees UbyOULINE MULTIPLIES CONFORMAL MATRICES 
poem (1), 6 (1) ,C (1) 
BO 20 I=1,NR 
DO 20 J=1,NC 
T=0.0D0 
PoetO K=1,NK 
IK=I+NR* (K-1) 
KJ=K*#NK*(J~-1) 
) T=T#+aA (IK) *8 (KJ) 
moi 1 +N R*L(.1- 1) 
} C(1Ig)=? 
RETUEN 
END 





a 





DS 


eo OR 


eee ee Se fe RGE ES Ge TES, XGES, WES) 
Meee Ose OF THiS SUBrOUNTIN® [5 TO SMOOTH 30 SAHPLTES OF 
Bear HYD GEV SMOOTHED ZSPIMATES OF THE LATEST DATA 
Bee. ESS ARE AS FOLLOWS: 
we ITS: 
=TITRE 
fee XR, EF SROR ESTIMATE 
Meepee ew = GHEE EXSSOR ESTIMATE 
eee Ss > 
Me OOLTHED XG ESROR ESTIMATE 
Meee rat D WEI1GHT ZPROF ESTIMATE 
Uso ORS eee et Cie Oe 5) yk (Oyo), Oo ee NN (2). 
Meee (2) CO MN (2) , DeMAX (2) ,K1,KO,CGAIN (2,5), BU3BLE, 
2 IDPINT, (2), FOR (2), FORSYS (3) , DII“2V (2) L975, 
: fee, oO), 8 (6,5) ,Ur(5), CORR CO wees (5) 7 OO NN 
Meee PP XOOPS, TX, JOISE (7) ,S °NDF (3) ,STNDM (7) ,X4S° (7) 
‘ too se SD Aa, POUR, SS I0 AR. , So DOP, TI VEC (30) ,xVEC (30), 


6 aVEC (30) 
ont MX, “4 
Seen vy eCLORS CONTAINING CATA TO BE SMOOTHED 
DO 10 T=1,29 
eet i +} 


Myec (1) =XVEC (I?) 
RBVEC (I) =aVEC{(I?P1} 
moma vec (1) =1Vic (I?Pi} 
mere (30)=XGtES7 
HVEC (320) =WZES7 
meee (30) = 
SUMT=6.0 


SUMNX=0.0 
SUMW=0.0 
eur 2=0.0 
SsJMx2=0.0 
SUMW2=0.0 
Sim X=0.0 
SUMTW=0.0 
bem2zo N=1,30 
Sree OUALT+T VEC (%) 
SUMX=SUMLZ+XV=EC EN) 
eee OUMat a VIC (N) 
pee 2—SUAT 2+ VET (i) **2 
Bee ace =SUMX2Z+XV EC (N) *¥¥2 
Bra 2=SUMA2+ eV ET (A) *F 2 
Bee AK OUMTX+T VEC (N) FKVEC (N)} 
ee A -SUMT A+ TV EC (IN) FBV EC (N) 

Re -SUMT/30. 
peo MY /30. 

=Spre/20. 
ite c- 5 J 2-30. FTN F*Z 
BGK 2=S UGX 2-30. KNEES 





PES Os 


meow 2 — SUNN 2~ 20. *ni** 2 
Peek = Si X- 40.5 NEE 
SPG i se-SU ST 4-20. STM eWN 
Me=SEGTY/SIGi2 
PR=SLGIA/STGT2 
Meto- i xt (LTH) +44 

Peo awe (7TH) +e 
eciUrN 

ENT 


BE 





MAIN 


See OUTTIERS OULP He og Op ae Ce Tree, OOP Me, 
Sees USFOUuTING IS CRLLED BeteEs 
Meee rn es AL EJUaATIVUdS SOLUTION 
Meee NS LON X (15) ,DOCRX¥ (15), 28 
mere ON DIS TAN C: aie (9% hg el Os 
een (2), OE YIN 42), DZ“ AX (2 
A GRE ee, Darl ro Marae Ooi Gs Me 


tO, A) Pee (a; =) ees tay P) 
Peel, 
mea, AGS, *GO, FLODAT, ELDR,TS 

oma 2C (30) 
mee K,NOLSE, MOG 


3 
fee =, LO2D, 1, .IISE(7),S7N 
3 


SLEMENTS OF TREE X VECTOF ARE AS 
(1) =DEOTH 
Meey=2h55 OF DESCENT 
Meey=OL°CH =2F0P 
Mippe=sch@s OF PITCH 
({S) =FARIQWATER PLANT DEFLECTIOV 
Miop=GhSEN PLANE DEFLECTION 
Meee 7OSNATD VELOCITY DEVIATION 
Misy=XG EPROP 
((9) =WETGHT FPRCE 
Mebeee=LEPTH FSTIMATS 
Memt-FATE OF DESCENT ESTIMATE 
Meee fTCH Z2R20R ISTIMATE 
Memey=2° TCH FATE ESTIMATE 
Meee G FRROR SSTI‘ATE 
Seo lS iGHT ERTOR SSTIMATE 

Pie =-FLD2AT 

Pepi. Li. 7S APT) FLDR=0. 

mm. G5.ISTCP)FLDOR=0. 

ATS SYSTEM AND 45ASUREMENT 


& 

meee Ok (1) =FO! 

Demo s- =i, 

Meee GAUSS (IK,SINOM 
Peemeaok (1)=X(I)+KOISE 
MeeeeCHANGE ORDEFS 

ee, Gl. 30.) XORD=180. 

eee 

fee Gl. 129.) YORD=H180. 

Men. G.. 200.) XOad= ee 

ee eGl «20 .) YORD= TE. 
Meee TON OF OPTIMaL CONT EOL 

Do 70 IT=1,2 

bee) -CGAIN (1,7) * (X™SE (1) -XC 


LLO 


Da? 
mMYMP OCT 


cae oe eet re mh 


see (esp 
2) , BFOF (9, 3),% (6,6) ,DZFMIN(2), 


g 

Weer OO ee fe 5) SI GOLe, 

92575(3) ,Dbet7 (3) ,LabD, 
COERZEC(6) ,ESER(4) , TPEINT 

NDF (3),STNDM{7) ,¥NSF (7) 

PAR TSTCP, VEC (30) ,XVEC (30) , 


, OOBMEY (IY 
a tale upon ee 


Riess L))) 


22) 
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O08 le) 


DO 79 J=2,5 
7 U(T) =U(L) #CGAIN (1, J) *X™SE (J) 
PMPOSITION CF CONSTRAINTS ON PLANES 
ro £0 Ir=1,2 
IDE FS +1 
TFP(U(I)) 10,50,30 
19 CONTINGE 
IF (X(IDEF) LE. 
TF(U(I).GF.DEM 
mee =D 247% (1) 
BO_TO 30 
30 CONTINUE 
Mere fLDE>).GrL.DE™4AX(I))GO TO 42 
Mit) ..2.3¢AX(1))GO TO 50 
U(I) =DRMAX (T) | 
GO TO SO 
47 X(IDEF)=DESéMIN(Z) 
ear TO 89 
43 X(IDEF)=DEPMAX (I) 
Boeui( it) = 
50 CCNTINUE 
EASURE UF FOP THE FILTER 
UF (1) =X%S2 (5) 
UF (2) =XMS® (5) 
UF (5) =XMSR(7) 
ULATE THE APPARENT & REAL ERRORS IN THE ESTIMATES 
ESER (1) =X (10) -X 452 (1) 
FSER (2) =X (11) -\ 432 (2) 
ESFR (3) =X (12) -2: SR(3) 
ee! oN) 
-you(s 
SES (6) =X (15) -¥) 5 
“ALCULATE THE CORRECTIONS FOR THE FILTER EQUATIONS 
DO &5 I=1,6 
BOEREC(I)=0.0 
DO 8&5 J=1,4 
OS COFFEC (I) =CORFFC(I) -K (I,J) * ESEP (J) 
fers e! PRINTING GF THE CUTPUT 
LF (T.LT,TPRINT) GO 50.3 


FOR 7% 


4IN(Z))GC 79 47 
Hy) Ge ees.) 


>A 


tt 
€) 


j 

a 

rrf 
i) 

a. 

4 
on 
it 


TT EMP= STHLE+1 
Meee — FECAL (IPR INT) -PRMT (3) /FLOAT(ITEMP) 
Meee eo. iM ATE CF CLATE OF TRI” 
Meee 502 (2, X fis yy, * (15),XGaS, ES) 
mee OLD. tt. 113}6C TC 8 
meer = (KO, 2001) 
i CI D=0 
eee UB=X(3)+SMR BLT 
eee nO, 2 ye il 1), U2) X (1) > X (2), 355808, % (4), 
ee), & (Oo) ae LS 
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be 
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rrt 
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rj 
4 
i 
tr 
ah 
LO 
Baa Sos ie pm eo i oe, 
3 
ry 
ee) 
13 
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Kad 0 3 


ery cs 
ea) 


= 
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estat 2: 


eam i 
rz 
= a4 
GJ CI" [Tt 2 


= 
| 
ent tn 


Ce hats) as 


Cy ee 


2 Ca 


ee bal bee 
=~ G) tyis 


Pa te CS 
"71 tal 

=~ 
— Ul 


ae 
= ) rg 


~ 
| 
j 


to ~J2t tr) Oe sD to ige7d id +] 


tYrjs 


3 


> 


= 

~ 
~J Qi #3 
am (NTS 


Varese 
—2 & 


be | 


<< 
al 
eae 


tls 
23 


- 
~— 
= 


ba] 


e({(yr) Gest) 


(13) Trt tu use 


oat 


rr 


are 
AT (1H 
FN 


x) 
ap) 


ee ease! BE 1: Ua 


as 
. 1 


as 
t= 
tv] 
a 
=_ 
= 
—_ 
ig 


LD e 
bettas te) 
NM 


eo as are 


202 
i 
: 
iy 


tr} tet Pry try 


* 
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FY 
ei=1,4) 
TD § 

<< rd 

es, § yy? ° 
Cee wa 
pase 

~ ~=(« 

- @ F 

2, 

% 
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MAIN 


Sor OUITINE FCT(T,X,DERX) 


eee POULINS CALCULATES THE RIGHT HAND SIDES OF THE 
Seer. EQURTIONS FOR THE RUNGE-KUTTA SOLUTION 


Dee eNSTON X(15),CER¥ (15) 
Meee ee tn (4, 9) GB (So, 2), BFOR (9, 7) ,K (6,6) ,DEFMIN(2), 


Meee (2) a MIN (2) , DACAAR (2) ,KIL,“O,CGAIN (2,6), 3UBBLE, 


i) Sa ca 3 (2) , FOR (3) , FORSYS (2), DTI EVR): GDL; 


3 AF (6,6),B?(5,5),UF(5), COPREC (6) ,ESER(5) , TPRINT 
Mees TED, XC2D,1X%, HOISE(7) ,SINDF (3) ,STNDM(7) ,X@SBE (7) 
5,MOG,XGP,XGO, FLDRAT, FLDR, TSTART, “STOP, TVEC (30), %V2C (20), 


We (30) 
meena K,NOLSE, MOG 


BATE EQUATIONS 


boy 30 I=1,? 


1,9 
Perel) =DERX (I) +A (TT, J) FX (J) 
2 


9 
DEPX (I) =DZRX (I) +B(I,J) *U (J) 
BO 30 J=1,3 
DERX (I) =DF2X (I) +BFOR (I,J) *FOR(J) 
DERX (8) =FLDP * (XGF-XGO-X (8) ) /(MOG+X (9) #1000000.) 
LERX (9) =FLD2/1000000. 


ese CS OUATIONS 


42 


DO G2 T[=1,6 

IPS=1I+9 

Meek (LPI) =CORREC (I) 

DO 41 J=1,5 

JP9=J+9 

DEFX (IP9%) =DEPX (IP9) +AP (I,J) *X (JPY) 
DO #2 J=1,5 

DEEX (IP9)=DERX (IP9) +BP(I,J) *UF(J) 
RETURN 

END 
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nN 


eee ee a, BVO, PO,70,8,K1,0T,NPOR ,NCX, IPNT) 
Meeetomees 4 MODIFICATION OF THE rae PrOme los ACCESS IT 
PACKAGE AT AIZ 


MeeyeLGHT “Msl.t. yo 


Meee N IS FOF THIS SUSROUTINE ARE AS FOLLOWS: 
fupeeeee ere COLFrTICTENT MATPICES 

Seems ( YVETRIX FROM THE PERFORMANCE INDEX 
Memes LAY EOSE OF P FaOM THE PERFORMANCE IMDEYX 


RO IS THE BOUNDARY CONDITION FOR THE PICCATI EQUATION 
meepommn= SCLUTION OF THE MATRIX RICCATI EQUATION 
KM IS THE GAINS “MATRIX 
DP IS THE TIME STED 
NEOW IS THE DIMENSION OF THE STATE VECTOR 
NCX IS THE NUMBER OF COLUANS IN THE 3 MATRIX 
TPNT IS THE PEINTE2?'S ALDRZSS 
MirPu; APGIEMEN =S AND RESOLT 
Meee A{4),8(1),00(7),P0(1),30(1) ,-8 (7) 
TEMPORA?Y STOPAGE AFRAYS 
BPB,ZA,AND 2G APE GFYERAL, ALL OTHERS ARE SYMMETRIC 
REAL BP8(100),2F (100) ,8D(100) ,RD7(100) ,RMAX (100) ,2 (100) 
meme 100) ,22 (100) ,FG (100) , KT(NCX, NR0W) 
NLCOPS=0 
10 H=DT/6.0 
NQ=NROW* (NROW+1) 72 
TIME SCALE EQUATION 
CAICULATE P=B*P*TRA (RB) 
FIFST GET R¥=P*7RA (B) 
DO 60 I=1,NC¥Y 
DO 60 J=1,‘1POR 
T=0.0 
DO 50 K=1,NCX 
TA=I+(K-1) *NCX 
IAT=K+ (I-1) *NCX 
IB=J+(%-1) *1V 204 
Oe T=T+ (PO (IA) +PC(ZAT))*B(IB) 
TC=1+(J-1) *NCX 
ee ORK {IC)=0.5*T 
THEN GET P=8*RK 


a 


Pee, HOIGH SESULT 25 SYAMPETREIC, DO FULL MATRIK FOP CONVENIENCE 


=0 

DO 80 J=1,N20W 

DO 80 I=1,NPOW 

T=0.0 

DO 70 K=1, NCY 

TA=I+ (K-1) ¥NPOW 

I3=K+(J-1) *NCX 
QO =T4+3(IA)*2* (1B) 





0 


MARS 


me=T C+] 
BeB(iC) =H*? 


Meese Pr eanD 0 TO SYMPPETPIC FORM AND FINISH TIME SCALING 


Ic=0 

DO 90 J=1,N20¥8 

Boe?O t=1,J 

TA=I+(J-1) *NFOW 

EB=J+(I-1) *NROR 

me=1C+4 

pemec) ~0.5* (°0(1TA)+R80 (1B) ) 
Q(IC) =0.5*H* (00 (LA) +Q2(TB)) 
°° : 

WRIT eats 009} 


FOPMAT(: ee ee a oe 28}, 


DO 190 I=1,NQ 
RMAX (I) =0.0 

RYNGE-KJTIA INTESGERATION 
Poae250 K=1,7 

DO 210 I=1,NOQ 

BT (I) =2 (1) 

G@ATL MRDOT(A,2EP3,0, BT,RA,RG,RDT 
DO 220 I=1,NQ 

RD (I) =RD7 (TZ) 

BT (I) =2 (1) +2.0*2D(T) 


att FR DOT (A, BPB,O, 27, RA,RG,RDT, 


DO 230 I=1,NQ 
beet) =n (5) +2.0*2 07 {T) 
Peet y—2D (7) +2.0"5D7 (1) 


eee AR DOT(A,223,0,812,kA,FG,RDT, 


DO 240 I=1,%N0 
BT (I) =R (1) +6.0*2DT (I) 
RD (I) =2D (I) +2.0*2DT (2) 


G@moL MRDOT (RN, BPS,0,BT,£4, 8G, RDT,H, 


eee ott? TS COMPLETE 


Meee ee 2 AND COMPUTE EPPOR NORE 


DRS=0.0 

am250 1=1,N9 
DR=ED (1) +297 (T) 
R(T)=2(1) +02 

DR=ABS (D2/07) 

2) 218 X* (02, SHAX (1) ) 


CONTINU 
COMPUTE AND DPINT CONTROLLER 
K=P*TPA(R) XP 
TIMPHTIMT+10.0*D7 

DO 280 I=1, NCX 

DO 270 J=1,NROW 

T=0.0 

DO 260 K=1,NROW 


‘ERROR! ,1T37, 


,H, NROR) 


H, 


q, 


GAINS 


NROW) 


NROW) 


NROW) 


MOONS ROBE 2 Gk Ns ts 





p07 


02 
1) 


wal 


Pe 


IA=I+ (X-1) *NCX 

GET BLE (K,J) FROM A SYMMETPIC MATRIX 

IF (K.LE.J) L820 *(J-1) /24+¥ 

TF (K.GT.J) LB=K*# (%-1) /240 

T=T+PK (TA) *2 (2B) 

mote, 3) =~ 

Bot) =T 

TP(I.£0.1) 42 IZTE(IPNT, 5001) TIME, DT, DRS, (RD(J) ,J=1,NROW) 
meen ( ie | 7PG11.8,10612.8,7(' *,40x%,9G12.8)) 
Moa. GT. 1) WRITE (IPNT, 5002) (2D(J) ,J=1,NROW) 
MemewAT (1H ,25X, 1P8G12.%,/(' *,&60X,8612.4)) 
Gee live 


Been aT = TF NOZT CONVEEGING 


73 


TP (TICS.2T.-120.0)GO TC 572 
PERHINATE I? DRS IS SMALL 
meoRs.G>t.0.001)GO oO 200 
age tO 572 

NLOOPS=NLOOPS+7 

Ten LOOPS.GT.1)GO TO $73 
DI=DT/2. 

Sa TO 10 

RETURN 

ENT 
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MAIN 


SUPPOUTING M2 DOT(A,EPB,Q,BT, 2A, 8S, RDT,H, NROW) 
SOPYRIGHT M.i.T. 1973 ALL RIGHTS RESERVED 


Me@omsouoSOUTINE IS CeLlLED SY MRS 
Mero. CTS INTEGER*2 (1 =N) 
epee (1) ,oeL (1) ,O(1),B5 (1) ,BACT), 2G (1), RDT (1) 
eee — = a mt) RR, 2S) =O {1 1)? (7, J) ¥O (5, K) *8 (5%, 1) 
Meee THAT 8 CONTAINS CURRENT VERSION GF R FOR RUNGE-KUTTA 
Perera Atl RA (A) *R USt FORMULA: 
Meet t, LJ=—R(l,K) FA (K,L) +A (CK, I) *R (K,L) 
Pere RePFo JSS FORMULSA: 
eget 1, LY=2(l,J3)*P (J, KY*R (K,L 

-— a Memeo. > EFA NAD RETSA(B)WINV (PP) FB=R*P SINCE P PRESET 
TJ=0 
BO,50 J=1, 
Doe SO T=1, 
maA=0.0 
TB=0.0 
IK=I* (1-1) 7/2 
KJ=(J-1) *N20O% 
DO SO K=1,NR0W 
Men. LE. 5)GO TO 20 
TK=IK+K=-% 
GOTO 30 

) IK=IK+1 
KJ=KJ+1 
Meee A+B. (TK) A (XI) 

) TB=TB+BT (1K) *BPB(KJ) 
TJ=IJ+1 
RG (IJ) =TB 

) RA(IJ) =TA*H 

_— a HOw COMPUTE NEW RDOT 
IJ=0 
DO 700 J=1,NRONW 
IL=(J-1) *NROW 
LI=J-NPOW 
Bo 00 I=1,J 

~ mm COMPUTE R&G*23 
TA=0.0 
KJ=J¥ (J-1) /2 
IK=I-N20W 
bo 90 K=1, {PON 
Meeks leE.J} GO TO 70 
KJ=KJ+K- 
GO TO 80 

) KJ=k J+1 
TK=IK+tN20W 

) me—lR+RG(IX)ABT (KI) 


NAOW 
NPOW 





0 


MRDOT 


ITJ=IJ+1 

IL=IL+1 

LI=LI4¢NROW 

moe (lJ) =TA-O (Id) -PA (IL) ~RA (LI) 
ee. URN 

ENC 


b23 


ah 








thes$4182 
Submarine control and dynamic trim estim | 
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